diff --git a/script/RobotModbus.py b/script/RobotModbus.py new file mode 100644 index 0000000..cd8abaf --- /dev/null +++ b/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) +