feat: add Modbus drivers for Jig, TPU, and RPU

drivers/modbus_device.py

@@ -0,0 +1,186 @@
+import struct
+import time
+
+from pymodbus.client import ModbusSerialClient
+from pymodbus.exceptions import ModbusException
+
+
+class ModbusDeviceError(Exception):
+    pass
+
+
+class ModbusConnectionError(ModbusDeviceError):
+    pass
+
+
+class ModbusReadError(ModbusDeviceError):
+    pass
+
+
+class ModbusWriteError(ModbusDeviceError):
+    pass
+
+
+class ModbusDevice:
+    FLOAT_VERIFY_TOLERANCE = 0.01
+
+    def __init__(self, port, device_name, registers=None, slave_id=0x10, baudrate=9600, timeout=3,
+                 retries=3, retry_delay_s=0.2):
+        self.port = port
+        self.device_name = device_name
+        self.registers = registers or {}
+        self.slave_id = slave_id
+        self.retries = retries
+        self.retry_delay_s = retry_delay_s
+        try:
+            self.client = ModbusSerialClient(port=port, baudrate=baudrate, parity="N", stopbits=1, bytesize=8, timeout=timeout)
+            if not self.client.connect():
+                raise ModbusConnectionError(f"failed to connect to {device_name} on {port}")
+        except ModbusException as exc:
+            raise ModbusConnectionError(f"error connecting to {device_name} on {port}") from exc
+
+    def _addr(self, label):
+        try:
+            return self.registers[label].address
+        except KeyError:
+            raise ModbusDeviceError(f"unknown register label for {self.device_name}: {label}")
+
+    def _retry(self, action):
+        last_exc = None
+        for attempt in range(self.retries):
+            try:
+                return action()
+            except (ModbusDeviceError, ModbusException) as exc:
+                last_exc = exc
+                if attempt < self.retries - 1:
+                    time.sleep(self.retry_delay_s)
+        raise last_exc
+
+    # ---- raw reads (retried on failure) ----
+
+    def read_coil(self, address):
+        def _do():
+            res = self.client.read_coils(address, count=1, device_id=self.slave_id)
+            if res.isError():
+                raise ModbusReadError(f"{self.device_name}: failed to read coil {address}")
+            return res.bits[0]
+        return self._retry(_do)
+
+    def read_holding_register(self, address):
+        def _do():
+            res = self.client.read_holding_registers(address, count=1, device_id=self.slave_id)
+            if res.isError():
+                raise ModbusReadError(f"{self.device_name}: failed to read holding {address}")
+            return res.registers[0]
+        return self._retry(_do)
+
+    def read_holding_register_float(self, address):
+        def _do():
+            res = self.client.read_holding_registers(address, count=2, device_id=self.slave_id)
+            if res.isError():
+                raise ModbusReadError(f"{self.device_name}: failed to read holding float {address}")
+            return struct.unpack(">f", struct.pack(">HH", res.registers[0], res.registers[1]))[0]
+        return self._retry(_do)
+
+    def read_input_register(self, address):
+        def _do():
+            res = self.client.read_input_registers(address, count=1, device_id=self.slave_id)
+            if res.isError():
+                raise ModbusReadError(f"{self.device_name}: failed to read input {address}")
+            return res.registers[0]
+        return self._retry(_do)
+
+    def read_signed_input_register(self, address):
+        val = self.read_input_register(address)
+        return val - 65536 if val > 32767 else val
+
+    def read_input_register_float(self, address):
+        def _do():
+            res = self.client.read_input_registers(address, count=2, device_id=self.slave_id)
+            if res.isError():
+                raise ModbusReadError(f"{self.device_name}: failed to read input float {address}")
+            return struct.unpack(">f", struct.pack(">HH", res.registers[0], res.registers[1]))[0]
+        return self._retry(_do)
+
+    # ---- writes, verified by reading the value back (whole write+verify retried on mismatch) ----
+
+    def write_coil(self, address, value, verify=True):
+        value = bool(value)
+
+        def _do():
+            res = self.client.write_coil(address, value, device_id=self.slave_id)
+            if res.isError():
+                raise ModbusWriteError(f"{self.device_name}: failed to write coil {address}")
+            if verify and self.read_coil(address) != value:
+                raise ModbusWriteError(f"{self.device_name}: write coil {address}={value} not verified")
+        self._retry(_do)
+
+    def write_holding_register(self, address, value):
+        value = int(value) & 0xFFFF  # two's-complement wrap so negative (signed) values fit the uint16 wire format
+
+        def _do():
+            res = self.client.write_register(address, value, device_id=self.slave_id)
+            if res.isError():
+                raise ModbusWriteError(f"{self.device_name}: failed to write holding {address}")
+            if self.read_holding_register(address) != value:
+                raise ModbusWriteError(f"{self.device_name}: write holding {address}={value} not verified")
+        self._retry(_do)
+
+    def read_signed_holding_register(self, address):
+        val = self.read_holding_register(address)
+        return val - 65536 if val > 32767 else val
+
+    def write_holding_register_float(self, address, value):
+        def _do():
+            reg1, reg2 = struct.unpack(">HH", struct.pack(">f", value))
+            res = self.client.write_registers(address, [reg1, reg2], device_id=self.slave_id)
+            if res.isError():
+                raise ModbusWriteError(f"{self.device_name}: failed to write holding float {address}")
+            readback = self.read_holding_register_float(address)
+            if abs(readback - value) > self.FLOAT_VERIFY_TOLERANCE:
+                raise ModbusWriteError(
+                    f"{self.device_name}: write holding float {address}={value} not verified (read back {readback})"
+                )
+        self._retry(_do)
+
+    # ---- generic label-based access (covers every register in self.registers) ----
+
+    def _register(self, label):
+        try:
+            return self.registers[label]
+        except KeyError:
+            raise ModbusDeviceError(f"unknown register label for {self.device_name}: {label}")
+
+    def read(self, label):
+        reg = self._register(label)
+        if reg.object_type == "coil":
+            return self.read_coil(reg.address)
+        if reg.object_type == "input":
+            if reg.data_type == "float":
+                return self.read_input_register_float(reg.address)
+            if reg.data_type == "int":
+                return self.read_signed_input_register(reg.address)
+            return self.read_input_register(reg.address)
+        if reg.object_type == "holding":
+            if reg.data_type == "float":
+                return self.read_holding_register_float(reg.address)
+            if reg.data_type == "int":
+                return self.read_signed_holding_register(reg.address)
+            return self.read_holding_register(reg.address)
+        raise ModbusDeviceError(f"{self.device_name}: unsupported object_type {reg.object_type!r} for {label!r}")
+
+    def write(self, label, value):
+        reg = self._register(label)
+        if reg.object_type == "coil":
+            self.write_coil(reg.address, value)
+            return
+        if reg.object_type == "holding":
+            if reg.data_type == "float":
+                self.write_holding_register_float(reg.address, value)
+            else:
+                self.write_holding_register(reg.address, value)
+            return
+        raise ModbusDeviceError(f"{self.device_name}: cannot write to object_type {reg.object_type!r} for {label!r}")
+
+    def close(self):
+        self.client.close()