def run(): IMU = arduino.IMU() start = (0, 0) end = (200, 200) grid, start, end = mapmaker.read() mToI = 1 posX = buggyinfo.Position(start[0], metersToIndex=mToI) posY = buggyinfo.Position(start[1], metersToIndex=mToI) posZ = buggyinfo.Position(0, initialV=1, metersToIndex=mToI) drawer = draw.Drawer(300, 300, 700, 700, start, end) while drawer.done is False: accel, gyro, dt = IMU.getIMU() accel.x = 0.01 accel.y = 0.01 dt = 1 posX.update(accel.x, dt) posY.update(accel.y, dt) posZ.update(accel.z, dt) drawer.updateNode(posX.convert(), posY.convert(), posZ.convert(), False) drawer.loop() drawer.drawParticle(int(posX), int(posY)) drawer.update()
def run(): # Init arduino properties, drawer, and map # while exit is false: # get accel and gyro # smooth with kalman # get position # update map (any new blocked nodes? Use image processing, ultrasonic, and other sensors) # plot astar to end # plot smooth path through next four nodes, extract angles (bezier curve, if angle is greater than allowed turn radius, eliminate node from path, or if its another buggy, slow the hell down!!!) # command angles to arduino until next node appears IMU = arduino.IMU() posX = buggyinfo.Position(0) posY = buggyinfo.Position(0) posZ = buggyinfo.Position(0) # constants n_time_steps = 10000 n_dim_state = 3 filtered_state_means = np.zeros((n_time_steps, n_dim_state)) filtered_state_covariances = np.zeros( (n_time_steps, n_dim_state, n_dim_state)) kf = make_filter() t = 0 #threshhold th = 1 while True: accel, gyro, magnet, dt = IMU.getIMU() # get accel and gyro # if (t == 0): # filtered_state_means[t] = numpy.array( # [accel.x, accel.y, accel.z]) # filtered_state_covariances[t] = numpy.eye(n_dim_state) if t < 200: pass elif t == 200: filtered_state_means[t] = numpy.array( [accel.x, accel.y, accel.z]) filtered_state_covariances[t] = numpy.eye(n_dim_state) print "passed" else: if abs(accel.x) < th: accel.x = 0 if abs(accel.y) < th: accel.y = 0 if abs(accel.z) < th: accel.z = 0 obs = numpy.array([accel.x, accel.y, accel.z]) results = kf.filter_update(filtered_state_means[t], filtered_state_covariances[t], obs, observation_offset = np.array([dt,dt,dt])) filtered_state_means[t + 1], filtered_state_covariances[ t + 1] = results x, y, z = filtered_state_means[t + 1] if (abs(x - filtered_state_means[t][0]) > 3): x = 0 if (abs(y - filtered_state_means[t][1]) > 3): y = 0 if (abs(z - filtered_state_means[t][2]) > 3): z = 0 if abs(x) < th: x = 0 if abs(y) < th: y =0 if abs(z) < th: z = 0 # get position posX.update(x, dt) posY.update(y, dt) posZ.update(z, dt) # magnet.roll, magnet.pitch, magnet.yaw = \ # buggyinfo.convertToHeading(magnet.x, magnet.y, magnet.z) print "%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s" % ( accel.x, accel.y, accel.z, x, y, z, int(posX), int(posY), int(posZ)) t += 1 arduino.arduino.stop()
def run(): # Init arduino properties, drawer, and map # while exit is false: # get accel and gyro # smooth with kalman # get position # update map (any new blocked nodes? Use image processing, ultrasonic, and other sensors) # plot astar to end # plot smooth path through next four nodes, extract angles (bezier curve, if angle is greater than allowed turn radius, eliminate node from path, or if its another buggy, slow the hell down!!!) # command angles to arduino until next node appears dcmotor = arduino.DCMotor(1) IMU = arduino.IMU() button1 = arduino.Button(3) sonar1 = arduino.Sonar(1) # grid, start, end = mapmaker.read() grid, start, end = mapmaker.make(30, 30), (0, 0), (5, 5) height, width = grid.shape[0:2] posX = buggyinfo.Position(start[0]) posY = buggyinfo.Position(start[1]) posZ = buggyinfo.Position(0) drawer = draw.Drawer(width, height, 700, 700, start, end, grid) dcmotor.driveForward(100) while drawer.done is False: # time0 = time.time() accel, gyro, magnet, dt = IMU.getIMU() # get accel and gyro # get position posX.update(accel.x, dt) posY.update(accel.y, dt) posZ.update(accel.z, dt) # magnet.roll, magnet.pitch, magnet.yaw = \ # buggyinfo.convertToHeading(magnet.x, magnet.y, magnet.z) # print "(%0.3f, %0.3f, %0.3f)\t(%0.3f, %0.3f, %0.3f)" % \ # (gyro.roll, gyro.pitch, gyro.yaw, # magnet.roll, magnet.pitch, magnet.yaw) # print "%s\t%s\t%s" % (accel.x, accel.y, accel.z) # print(sonar1.getDistance()) # if button1.wasPressed(): # dcmotor.reverse() # update map (any new blocked nodes? Use image processing, ultrasonic, and other sensors) # ignore until below routines are finished # plot astar to end # path, pathInfo, plotTime = astar.search(grid, (posX, posY), end) # plot smooth path through next four nodes, extract angles # command angles to arduino until next node appears drawer.loop() drawer.drawParticle(int(posX), int(posY)) drawer.update() # print time.time() - time0 drawer.deinit() arduino.arduino.stop()