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
