Startup

script/RobotModbus.py

@@ -0,0 +1,46 @@
+class RobotModbus(DeviceBase):
+    def __init__(self, name):
+         DeviceBase.__init__(self, name)
+         robot_req.write(0)
+
+    def execute(self, command, *argv):
+        if robot_req.read() != 0:
+            raise Exception("Ongoing command")
+        if robot_ack.read() != 0:
+            raise Exception("Robot is not ready")
+        robot_cmd.write(command)
+        args = [0] * robot_args.size
+        index = 0        
+        for arg in argv:
+            args[index] = arg
+            index = index + 1
+            if index == robot_args.size:
+                raise Exception("Invalid number of arguments")
+        robot_args.write(to_array(args, 'i'))
+        try:           
+            self.request()         
+            err = robot_ack.take()            
+            if err == 1:
+                ret = robot_ret.read()            
+                return ret
+            if err == 2:
+                raise Exception("Invalid command: " + str(command))                
+            raise Exception("Unknown error: " + str(err)) 
+        finally:
+            self.cancel_request()
+    
+    def request(self):
+        robot_req.write(1)
+        while robot_ack.read() == 0:
+            time.sleep(0.001)
+
+    def cancel_request(self):
+        robot_req.write(0)
+        while robot_ack.read() != 0:
+            time.sleep(0.001)
+        
+    def mount(self, puck, sample):
+        return self.execute('1', '1', puck, sample)        
+
+add_device(RobotModbus("robot_mb"), force = True)                
+