mapNotes = 'This is a map of the maze.'
hazardNotes = 'This maps hazard information.'
origin = [0, 4] #stored in grid points. [x,y]
map_size = 17 #length of each side on the map
prev_encoder = 0
#initialize the map and the origin
map = np.zeros((map_size, map_size), np.int8)
currLoc = [[origin[0], origin[1]],
[origin[0] * conversion,
origin[1] * conversion]] #stored in [grid points, cm]
hazards = []
hazardMap = []
#IMU Setup
accelx = genWindow(mag.width, 0)
accely = genWindow(mag.width, 0)
accelz = genWindow(mag.width, 0)
#sensor setup
gyro_port = BP.PORT_4
BP.set_sensor_type(gyro_port, BP.SENSOR_TYPE.EV3_GYRO_ABS_DPS)
us_front = BP.PORT_3
BP.set_sensor_type(us_front, BP.SENSOR_TYPE.EV3_ULTRASONIC_CM)
us_right = 6
us_left = 5
listIR = np.zeros(8)
indexIR = 0
#sensor warmup
flag = 1
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()
baseAccelDataX = WindowFilterDyn(
accelx, dly,
InvGaussFilter(adv, baseAccelData['x'], biases[0], std[0], count))
#test case for right now
def distanceUsingAccelX(accelX, dT):
accelX = abs(accelX)
baseVel = accelX * dT
baseDist = baseVel * dT * dT
print(accelX)
from IMUFilters import KalmanFilter
from IMUFilters import FindSTD
from IMUFilters import InvGaussFilter
mpu9250 = MPU9250()
feele = open("dataSet.csv","w")
#Parameters
width=1
depth=100
dly=0.01
adv = True
#/////////
accelx=genWindow(width,0)#Can play with width to adjust system
accely=genWindow(width,0)
accelz=genWindow(width,0)
gyrox=genWindow(width,0)#Can play with width to adjust system
gyroy=genWindow(width,0)
gyroz=genWindow(width,0)
magx=genWindow(width,0)#Can play with width to adjust system
magy=genWindow(width,0)
magz=genWindow(width,0)
flter=[[0.7,1.0],[0.7,1.0],[0.7,1.0],[0.7,1.0],[0.7,1.0],[0.7,1.0],[5,2],[5,2],[5,2]]
# [r,q]Will need to play with each filter value
#
# A note on Kalman filter parameters: Each of these parameters represents the process and system noise respectively.
# In order to adjust the output of the Kalman filter, each of these parameters can be modified. Do not set these
# parameters to zero, because a Kalman filter actually needs a little bit of noise, or it becomes unstable and
# effectively useless.