def getData():
gyro = mpu.readGyro()
gx = gyro['x']
gy = gyro['y']
gz = gyro['z']
touch = BP.get_sensor(one)
x,y,z = AvgCali(mpu, 5, 0.1)
gyro = {'x':(gx - x), 'y':(gy - y), 'z':(gz - z)}
return(touch, gyro)
from IMUFilters import InvGaussFilter
import grovepi
bus = smbus.SMBus(1)
mpu9250 = MPU9250()
x = 1
width = 10
depth = 10
dly = 0.01
adv = True
biases = AvgCali(mpu9250, depth, dly)
state = [[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[0, 0, 0, 0, 0, 0, 0, 0,
0]] #Estimated error (p) and measurement state (x)
out = [0, 0, 0, 0, 0, 0, 0, 0, 0]
std = FindSTD(biases, mpu9250, dly)
pick = 1 #1 uses window filter, anything else uses Kalman
count = 3 #Number of standard deviations used for filtering
t0 = time.time()
accelx = genWindow(width, 0) #Can play with width to adjust system
accely = genWindow(width, 0)
accelz = genWindow(width, 0)
baseAccelData = mpu9250.readAccel()