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