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