26 lines
587 B
Python
26 lines
587 B
Python
if robot_req.read() != 0:
|
|
raise Exception("Ongoing command")
|
|
if robot_ack.read() != 0:
|
|
raise Exception("Robot is not ready")
|
|
robot_cmd.write(1)
|
|
args = [0,0,0,0,0,0]
|
|
|
|
robot_args.write(to_array(args, 'i'))
|
|
|
|
|
|
robot_req.write(1)
|
|
while robot_ack.read() == 0:
|
|
time.sleep(0.001)
|
|
|
|
err = robot_ack.take()
|
|
if err == 1:
|
|
ret = robot_ret.read()
|
|
print ret
|
|
if err == 2:
|
|
print ("Invalid command: " + str(command))
|
|
print ("Unknown error: " + str(err))
|
|
robot_req.write(0)
|
|
while robot_ack.read() != 0:
|
|
time.sleep(0.001)
|
|
|