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 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