예제 #1
0
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)
예제 #2
0
def mainloop():
    
    log("Starting command server")
    HOST = get_local_ipv4_address()
    PORT = roboconfig.command_port
    command_server = SocketServer.UDPServer((HOST,PORT),MyUDPHandler)
    command_server.serve_forever()
def process_control_line(line):
    """Processes a line from the RoboMagellen control file."""

    # skip comments and blank lines
    if line[0] == '#':
        return None
    if line == "":
        return None
    re_anything = re.compile('\S+')
    if re_anything.match(line) == None:
        return None

    # determine and process line type
    line = line.rstrip()
    if line[0:4] == "DRIV":
        return process_driv_directive(line)
    elif line[0:4] == "GOTO":
        return process_goto_directive(line)
    elif line[0:4] == "CONE":
        return process_cone_directive(line)
    elif line[0:4] == "STOP":
        return process_stop_directive(line)
    elif line[0:4] == "STER":
        return process_ster_directive(line)
    elif line[0:4] == "POWR":
        return process_powr_directive(line)
    elif line[0:4] == "WAIT":
        return process_wait_directive(line)
    else:
        log("Undefined directive " + line[0:4] +" in line: " + line)
        return False
def initialize():
    log("Sensor initialization started")
    compass_thread = CompassThread()
    compass_thread.start()
    sensor_thread = SensorThread()
    sensor_thread.start()
    log("Sensor initialization completed")
    return True
 def run(self):
     while self.run_flag == True:
         global compass_data
         self.ser.flushInput() # Discard readings buffered during sleep
         data = self.ser.readline()
         m = self.compass_re.match(data)
         if m:
             compass_data = int(m.group(1)) + 0.1 * int(m.group(2))
         else:
             log("robosensors - recieved invalid compass data: %s" % data)
         time.sleep(roboconfig.compass_read_delay)
     log("Compass thread terminated")
예제 #6
0
def initialize():
    try:
        robocomms.initialize()
        robopower.initialize()
        robocamera.initialize()
        robosensors.initialize()
        robonavigation.initialize()
    except RobotError as e:
        log("RobotException: " + e.value)
        log("Exception occurred in initialization - ending")
        robopower.halt()
        exit(0)
    return
def process_control_file(filename):
    """Parses the RoboMagellan control file. Returns a data structure with
    the route details if sucessful, otherwise NULL if an error is
    found"""

    log("Processing control file: " + filename)
    error_count = 0
    control_data = []

    # Process the file
    f = open(filename)
    while True:
        line = f.readline()
        log(">>> " + line.rstrip())
        if not line: break
        result = process_control_line(line)
        if result == False:
            error_count = error_count + 1
        elif result == None:
            pass
        else:
            control_data.append(result)
    f.close()

    # Return the control data or None to the caller
    if error_count == 1:
        log("Control file processed, " + str(error_count) + " error.")
    else:
        log("Control file processed, " + str(error_count) + " errors.")

    if error_count == 0:
        return control_data
    else:
        raise RobotError('Control file error')
예제 #8
0
def initialize():
    try:
        control_data = robocontrol.process_control_file(sys.argv[1])
        robocomms.initialize()
        robopower.initialize()
        robocamera.initialize()
        robosensors.initialize()
        robonavigation.initialize()
    except RobotError as e:
        log("RobotException: " + e.value)
        log("Exception occurred in initialization - ending")
        robopower.halt()
        exit(0)
    return control_data
    def run(self):
        while self.run_flag == True:
            global ping_data, bumper_status
            self.ser.flushInput() # Discard readings buffered during sleep
            self.ser.readline()
            # Read five lines of data to get complete input
            for i in range(5):
                data = self.ser.readline().lower().strip()
                if data.find('bumper') == 0:
                    if data.find('on') > 0:
                        bumper_status = True
                    elif data.find('off') > 0:
                        bumper_status = False
                    else:
                        log("robosensors - recieved invalid bumper data: %s" % \
                                data)
                elif data.find('sensor') == 0:
                    m = self.sensor_re.match(data)
                    if m:
                        ping_data[int(m.group(1))-1] = int(m.group(2))
                    else:
                        log("robosensors - recieved invalid sensor data: %s" % \
                                data)
                else:
                    log("robosensor - received invalid data %s" % data)

            time.sleep(roboconfig.sensor_read_delay)
        log("Sensor thread terminated")
예제 #10
0
    def set_speed(self, k):

        k_val = 17 - abs(k)

        if k < 0:
            right_motor = "1,0,1,%s\n" % k_val 
            left_motor =  "2,0,0,%s\n" % k_val
        elif k == 0:
            right_motor = "1,1,0,0\n"
            left_motor  = "2,1,0,0\n"
        else:
            right_motor = "1,0,0,%s\n" % k_val
            left_motor  = "2,0,1,%s\n" % k_val

        log("Right: %s" % right_motor)
        log("Left:  %s" % left_motor)

        self.ser.write(right_motor)
        self.ser.write(left_motor)
예제 #11
0
def main():
    log("RoboMagellan 2012 started")
    initialize()
    time.sleep(1)  # Allow time for processing threads to start
    try:
        mainloop()
    except:
        log("*** ERROR *** A fatal error has occured *** ERROR ***")
        log(str(traceback.format_exc()))
    else:
        log("RoboMagellan 2012 ended - Champagne Time!")

    # Stop the robot
    robopower.halt()
    
    # Terminate running threads
    for thread in threading.enumerate():
        if thread.name != 'MainThread':
            log("Terminating thread: " + str(thread))
            thread.stop()
예제 #12
0
    def run(self):
        while True:
            global current_speed, target_speed

            # If necessary, bring current speed to target
            if current_speed != target_speed:
                if current_speed < target_speed:
                    values = range(current_speed+1, target_speed+1)
                else:
                    values = range(current_speed-1, target_speed-1, -1)
                for k in values:
                    log("Setting speed to %d" % k)
                    self.set_speed(k)
                    current_speed = k
                    time.sleep(roboconfig.power_delay)

            if self.run_flag == False:
                break
            time.sleep(roboconfig.power_delay)

        log("Power thread terminated")    
예제 #13
0
def log_robot_status():
    ping = robosensors.get_sensors()
    touch_sensor = robosensors.get_bumper()
    log(">> Speed=%d" % robopower.get_power())
    log(">> Compass=%3.2f" % robosensors.get_compass())
    log(">> Touch=%s, Ping1=%d, Ping2=%d, Ping3=%d" % \
            (touch_sensor, ping[0], ping[1], ping[2]))
예제 #14
0
def process_cone_directive(line):
    # Parse Cone data
    cone_re = re.compile('CONE\s+(\d\d)(\d\d).(\d+)\s+(\d\d\d)(\d\d).(\d+)')
    m = cone_re.search(line)
    if not m:
        log("Invalid CONE directive in line: " + line)
        return False

    # Validate and calculate latitude
    lat_deg = int(m.group(1))
    lat_min = int(m.group(2))
    if lat_min > 59:
        log("Minutes must be less than 60 in latitude specification in line: " + line)
        return False
    lat_min = lat_min + int(m.group(3))/1000.0
    lat_deg = lat_deg + lat_min/60.0
    
    # Validate and calculate longitude
    lon_deg = int(m.group(4))
    lon_min = int(m.group(5))
    if lon_min > 59:
        log("Minutes must be less than 60 in longitude specification in line: " + line)
        return False
    lon_min = lon_min + int(m.group(6))/1000.0
    lon_deg = lon_deg + lon_min/60.0
    
    return {'command': "CONE", 'latitude': lat_deg, 'longitude': lon_deg}
예제 #15
0
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
예제 #16
0
def re_sight_cone():
    
    log("Re-sighting cone...")

    # Try a few times
    for i in range(roboconfig.re_sight_attempts):
        (camera_X, camera_Y, camera_distance) = robocamera.get_data()
        if camera_distance < roboconfig.max_camera_distance:
            log("...sucess!")
            return True
        else:
            time.sleep(re_sight_sleep_time)
        
    # Couldn't find it - return False        
    log("...failed")        
    return False
예제 #17
0
def speed(speed_value):
    assert(-16 <= speed_value <= 16)
    global target_speed
    log("Old target speed = %d" % target_speed)
    target_speed = speed_value
    log("New target speed = %d" % target_speed)
예제 #18
0
 def stop(self):
     log("Terminating sensor thread")
     self.run_flag = False
예제 #19
0
 def stop(self):
     log("Terminating compass thread")
     self.run_flag = False
예제 #20
0
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)
예제 #21
0
def initialize():
    log("Power initialization started")
    power_thread = PowerThread()
    power_thread.start()
    log("Power initialization completed")
    return True