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)
import mpu9150
import time
from math import *
mpu9150.mpuInit(1, 10, 4)
mpu9150.setMagCal('magcal.txt')
mpu9150.setAccCal('accelcal.txt')
while True:
i = mpu9150.mpuRead()
if i >= 0:
#print mpu9150.getFusedEuler()
mag = mpu9150.getCalMag()
bearing = atan2(mag[0], mag[1]) * (180.0 / pi)
if bearing < 0:
bearing = bearing + 360
print bearing
time.sleep(0.1)
def initImu(self, acc_cal_file, mag_cal_file): mpu9150.mpuInit(1, 10, 4) mpu9150.setMagCal(mag_cal_file) mpu9150.setAccCal(acc_cal_file)
def __init__(self, update_rate):
self.mpu_valid = -1.0
while self.mpu_valid < 0:
time.sleep(0.5)
self.mpu_valid = mpu9150.mpuInit(1, update_rate, 4)
def initImu(self, acc_cal_file, mag_cal_file): mpu9150.mpuInit(1, 10, 4) mpu9150.setMagCal(mag_cal_file) mpu9150.setAccCal(acc_cal_file)
import mpu9150
import time
from math import *
mpu9150.mpuInit(1, 10, 4)
mpu9150.setMagCal('magcal.txt')
mpu9150.setAccCal('accelcal.txt')
while True :
i = mpu9150.mpuRead()
if i >= 0:
#print mpu9150.getFusedEuler()
mag = mpu9150.getCalMag()
bearing = atan2(mag[0], mag[1])*(180.0/pi)
if bearing < 0 :
bearing = bearing +360
print bearing
time.sleep(0.1)
def __init__(self, update_rate):
self.mpu_valid = -1.0
while self.mpu_valid < 0:
time.sleep(0.5)
self.mpu_valid = mpu9150.mpuInit(1, update_rate, 4)