Startup

script/test/TestRobotCmds2.py

@@ -0,0 +1,25 @@
+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)
+