def nav_loop(): robot = UgvPlatform() #robot.setColorLut('lut_file.lut') #robot.initImu('accelcal.txt', 'magcal.txt') mpu_valid = -1 print "waiting for GPS fix" gps_service = GpsService() current_pos = gps_service.getPosition() while not current_pos.valid: time.sleep(1) current_pos = gps_service.getPosition() print current_pos.valid while mpu_valid < 0: time.sleep(1.0) mpu_valid = mpu9150.mpuInit(1, 10, 4) print mpu_valid mpu9150.setMagCal('./magcal.txt') mpu9150.setAccCal('./accelcal.txt') #time.sleep(4) wp = StaticWayPointProvider() controller = Controller() threads.append(controller) #gps_service.start() robot.setSteeringFailSafe(0.0) robot.setSpeedFailSafe(ESC_ARM_ANGLE) robot.resetWatchdog() robot.setSteeringAngle(0.0) robot.setSpeed(0) while True: robot.resetWatchdog() target_pos = wp.getCurrentWayPoint() sensors = populateSensorMap(robot, gps_service) current_pos = sensors[Controller.gps_key] imu_sample = sensors[Controller.imu_key] if imu_sample[0] < 0: time.sleep(0.05) continue #print "lat : "+str(current_pos.lat)+" lon :"+str(current_pos.lon)+"imu:"+str(imu_sample) # continue cmd = controller.getCommand(sensors) if cmd == None: break print str(cmd) if cmd.has_key(Controller.next_waypoint_key ) and cmd[Controller.next_waypoint_key] != None and cmd[ Controller.next_waypoint_key] == 1: wp.getNextWaypoint() else: robot.setSteeringAngle(cmd[Controller.steering_key]) robot.setSpeed(cmd[Controller.speed_key]) time.sleep(0.100) robot.setSteeringAngle(0.0) robot.setSpeed(0) print "Shutdown ESC then quit programm (for safety reason)" while True: time.sleep(1)
def nav_loop(): robot = UgvPlatform() #robot.setColorLut('lut_file.lut') #robot.initImu('accelcal.txt', 'magcal.txt') mpu_valid = -1 print "waiting for GPS fix" gps_service = GpsService() current_pos = gps_service.getPosition() while not current_pos.valid: time.sleep(1) current_pos = gps_service.getPosition() print current_pos.valid while mpu_valid < 0: time.sleep(1.0) mpu_valid = mpu9150.mpuInit(1, 10, 4) print mpu_valid mpu9150.setMagCal('./magcal.txt') mpu9150.setAccCal('./accelcal.txt') #time.sleep(4) wp = StaticWayPointProvider() controller = Controller() threads.append(controller) #gps_service.start() robot.setSteeringFailSafe(0.0) robot.setSpeedFailSafe(ESC_ARM_ANGLE) robot.resetWatchdog() robot.setSteeringAngle(0.0) robot.setSpeed(0) while True: robot.resetWatchdog() target_pos= wp.getCurrentWayPoint() sensors = populateSensorMap(robot, gps_service) current_pos = sensors[Controller.gps_key] imu_sample = sensors[Controller.imu_key] if imu_sample[0] < 0 : time.sleep(0.05) continue #print "lat : "+str(current_pos.lat)+" lon :"+str(current_pos.lon)+"imu:"+str(imu_sample) # continue cmd = controller.getCommand(sensors) if cmd == None: break print str(cmd) if cmd.has_key(Controller.next_waypoint_key) and cmd[Controller.next_waypoint_key] != None and cmd[Controller.next_waypoint_key] == 1: wp.getNextWaypoint() else: robot.setSteeringAngle(cmd[Controller.steering_key]) robot.setSpeed(cmd[Controller.speed_key]) time.sleep(0.100) robot.setSteeringAngle(0.0) robot.setSpeed(0) print "Shutdown ESC then quit programm (for safety reason)" while True: time.sleep(1)
sensor_map = {} sensor_map[Controller.imu_key] = (euler, gyro) sensor_map[Controller.gps_key] = gps_service.getPosition() sensor_map[Controller.blobs_key] = robot.getBlobPos() #TODO: add odometry and blobs return sensor_map if __name__ == "__main__": blink = 0xAA robot = AvcPlatform() #robot.setColorLut('lut_file.lut') #robot.initImu('accelcal.txt', 'magcal.txt') wp = StaticWayPointProvider() #gps_service = GpsService() controller = Controller() #gps_service.start() robot.setServoFailSafe(ESC_SERVO, ESC_ARM_ANGLE) robot.setServoFailSafe(STEERING_SERVO, 0.0) robot.resetWatchdog() robot.setServoAngle(STEERING_SERVO, 0.0) robot.setServoAngle(ESC_SERVO, ESC_ARM_ANGLE) try: while True: robot.setLeds(blink) robot.resetWatchdog() #target_pos= wp.getCurrentWayPoint() #current_pos = gps_service.getPosition() #sensors = populateSensorMap(robot, gps_service) sensors = {} #if len(sensors) == 0:
sensor_map = {} sensor_map[Controller.imu_key] = (euler, gyro) sensor_map[Controller.gps_key] = gps_service.getPosition() sensor_map[Controller.blobs_key] = robot.getBlobPos() #TODO: add odometry and blobs return sensor_map if __name__ == "__main__": blink = 0xAA robot = AvcPlatform() #robot.setColorLut('lut_file.lut') #robot.initImu('accelcal.txt', 'magcal.txt') wp = StaticWayPointProvider() #gps_service = GpsService() controller = Controller() #gps_service.start() robot.setServoFailSafe(ESC_SERVO, ESC_ARM_ANGLE) robot.setServoFailSafe(STEERING_SERVO, 0.0) robot.resetWatchdog() robot.setServoAngle(STEERING_SERVO,0.0) robot.setServoAngle(ESC_SERVO,ESC_ARM_ANGLE) try: while True: robot.setLeds(blink) robot.resetWatchdog() #target_pos= wp.getCurrentWayPoint() #current_pos = gps_service.getPosition() #sensors = populateSensorMap(robot, gps_service) sensors = {} #if len(sensors) == 0: