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)