def mainloop(control_data):
for command in control_data:
log("Processing command: " + repr(command))
if command['command'] == 'CONE':
navigate_to(command['latitude'], command['longitude'], mode='cone')
elif command['command'] == 'GOTO':
navigate_to(command['latitude'], command['longitude'], mode='goto')
elif command['command'] == 'DRIV':
log("DRIV command not yet implemented")
elif command['command'] == 'STOP':
robopower.stop()
return
elif command['command'] == 'POWR':
robopower.power(command['power'])
elif command['command'] == 'STER':
robopower.steer(command['steering'])
elif command['command'] == 'WAIT':
time.sleep(command['sleeptime'])
else:
# Should never happen: robocontrol should catch, but hey...
log("Error in mainloop: invalid command " + command['command'])
robopower.halt()
raise RobotError
def navigate_to(latitude, longitude, mode='cone'):
assert (mode == 'cone') or (mode == 'goto')
log("Navigate to lat: " + str(latitude) + ", lon: " + str(longitude))
log("Navigate to mode=" + mode)
task_complete = False
while task_complete == False: #TODO - probably need to time out
# Get camera, compass heading, and GPS data
(camera_X, camera_Y, camera_distance) = robocamera.get_data()
log("Navigate to: camera_X = %03d, camera_Y = %03d, distance = %1.2f" \
% (camera_X, camera_Y, camera_distance))
current_heading = robosensors.get_compass()
log("Navigate to: current heading = %d" % current_heading)
(gps_distance, heading_to_target) = \
robonavigation.vector_to_target(latitude, longitude)
log("Navigate to: target distance = %2.2f, vector = %d" % \
(gps_distance, heading_to_target))
# Test to see if we are at the GPS waypoint (within the radius of
# error). We we are and we're in goto mode, we're done. Otherwise
# switch to looking for the cone with the camera.
if gps_distance < roboconfig.gps_radius:
log("GPS waypoint found")
if mode == 'goto':
log("Goto mode - task complete")
return
else:
# If we can see the cone at this point, go and locate
# it. If we can't, keep going until we can see it.
#
# Note that locate_cone() may fail if the camera loses
# sight of the cone. If that happens, we drop back to
# locating the GPS waypoint again
if camera_distance < roboconfig.max_camera_distance:
if locate_cone() == True:
return
# Outside of GPS range - set the steer angle and drive/turn
# towards the cone
steer_angle = \
robonavigation.get_steer_angle(current_heading, heading_to_target)
log("Navigate to: steer_angle = " + str(steer_angle))
robopower.speed(5)
robopower.steer(steer_angle/180.0)
# Slow things down!
time.sleep(3)
def locate_cone():
# First stop the robot
robopower.speed(0)
# Now drive slowly towards the cone
while True:
(camera_X, camera_Y, camera_distance) = robocamera.get_data()
# If we lose sight of the cone try and find it again
if camera_distance > roboconfig.max_camera_distance:
if re_sight_cone() == False:
return False # failed to re-sight: go back to GPS
else:
continue
# Steer toward the cone
robopower.steer(int(camera_X/32.0-10.0))
robopower.speed(1)
# Check the touch sensor
if robosensors.touch_down():
return True
