def process_command(command):
# Ignore blank lines
if command == "":
return
# Speed command
if command[0:5].lower() == 'speed':
speed_re = re.compile('speed\s+([+|-]*)(\d+)')
match = speed_re.search(command)
if not match:
log("Invalid speed command, usage is: speed <integer>")
return
speed = int("%s%s" % (match.group(1), match.group(2)))
if speed < -16 or speed > 16:
log("Invalid speed: must be -16 <= speed <= 16")
return
robopower.speed(speed)
log("Speed set to %d" % speed)
return
# Stop command
if command[0:4].lower() == 'stop':
robopower.stop()
log("Robot stopped")
return
# No match - must be an invalid command
log("Invalid command: %s" % command)
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