def handler(data):
cmd,arg = protocol.parseData(data)
log.log("Received cmd " + str(cmd))
log.log("With arg " + str(arg))
if cmd == protocol.MOVE_CMD:
if arg == protocol.MOVE_CMD_BWD:
wheels.bwd()
elif arg == protocol.MOVE_CMD_BWD_R:
wheels.bwdr()
elif arg == protocol.MOVE_CMD_BWD_L:
wheels.bwdl()
elif arg == protocol.MOVE_CMD_FWD:
wheels.fwd()
elif arg == protocol.MOVE_CMD_FWD_R:
wheels.fwdr()
elif arg == protocol.MOVE_CMD_FWD_L:
wheels.fwdl()
elif arg == protocol.MOVE_CMD_ROTL:
wheels.rotl()
elif arg == protocol.MOVE_CMD_ROTR:
wheels.rotr()
elif arg == protocol.MOVE_CMD_STOP:
wheels.stop()
else:
wheels.stop()
elif cmd == protocol.SETSPEED_CMD:
wheels.setSpeed(int(arg))
elif cmd == protocol.SHUTDOWN_CMD:
wheels.outit()
else:
log.log("Unknown command: " + str(cmd))
def awake():
wheels.init()
wheels.setSpeed(50)
log.log("Wheels inited.")
log.log("Starting receive loop.")
udp_rx.go(handler)
wheels.fwd()
elif arg == protocol.MOVE_CMD_FWD_R:
wheels.fwdr()
elif arg == protocol.MOVE_CMD_FWD_L:
wheels.fwdl()
elif arg == protocol.MOVE_CMD_ROTL:
wheels.rotl()
elif arg == protocol.MOVE_CMD_ROTR:
wheels.rotr()
elif arg == protocol.MOVE_CMD_STOP:
wheels.stop()
else:
wheels.stop()
elif cmd == protocol.SETSPEED_CMD:
wheels.setSpeed(int(arg))
elif cmd == protocol.SHUTDOWN_CMD:
wheels.outit()
else:
log.log("Unknown command: " + str(cmd))
def awake():
wheels.init()
wheels.setSpeed(50)
log.log("Wheels inited.")
log.log("Starting receive loop.")
udp_rx.go(handler)
if __name__ == "__main__":
log.log("Robot awakening.")
awake()
