precommit

hardware/t3r_driver.py

@@ -0,0 +1,287 @@
+"""T3R Stepper Controller driver for ScanEngine-3.
+
+Qt-based driver that owns the serial connection and an internal reader QThread.
+All events arrive as Qt signals; all commands are fire-and-forget writes.
+Create in the main (GUI) thread; no additional thread management required.
+
+Gear train (stage rotation via GR-axis, ch3):
+    Motor  →  10T pinion  →  30T idler  →  125T index gear (stage)
+    Ratio motor:stage = 125/10 = 12.5
+"""
+
+from __future__ import annotations
+
+import threading
+
+import serial
+from PyQt6.QtCore import QObject, QThread, QTimer, pyqtSignal
+
+from . import t3r_protocol as proto
+
+
+class _T3RReader(QThread):
+    """Blocking read loop — runs on its own QThread."""
+
+    frame = pyqtSignal(int, bytes)         # (cmd, payload) for each valid frame
+    finished_reason = pyqtSignal(str)      # "" = clean stop, else I/O error string
+
+    def __init__(self, ser: serial.Serial):
+        super().__init__()
+        self._ser = ser
+        self._running = True
+        self._parser = proto.FrameParser()
+
+    def stop(self):
+        self._running = False
+
+    def run(self):
+        reason = ""
+        while self._running:
+            try:
+                data = self._ser.read(256)
+            except Exception as exc:
+                reason = str(exc) or exc.__class__.__name__
+                break
+            if data:
+                for cmd, payload in self._parser.feed(data):
+                    self.frame.emit(cmd, payload)
+        self.finished_reason.emit(reason)
+
+
+class T3RDriver(QObject):
+    """Qt-based driver for the T3R four-channel stepper controller.
+
+    Usage::
+
+        driver = T3RDriver()
+        driver.handshake_ok.connect(lambda pv, fw, nc: print("connected"))
+        driver.info_updated.connect(on_info)
+        driver.connect("/dev/ttyUSB0")
+        driver.move(0, steps=3200, velocity=8000, accel=4000)
+    """
+
+    CHANNEL_NAMES = ["T-axis (focus)", "Axis 1", "Axis 2", "GR-axis"]
+    GR_AXIS_CH = 3
+
+    # Gear train constants
+    MOTOR_FULL_STEPS_PER_REV = 200
+    GEAR_TEETH_MOTOR = 10
+    GEAR_TEETH_STAGE = 125  # idler is 30T but does not change ratio
+
+    # ── Signals ───────────────────────────────────────────────────────────────
+
+    port_opened = pyqtSignal()              # serial port open; PING sent
+    handshake_ok = pyqtSignal(int, int, int)  # proto_ver, fw_ver, num_channels
+    disconnected = pyqtSignal(str)          # reason ("" = user-initiated)
+
+    info_updated = pyqtSignal(int, object)        # ch, proto.Info
+    drv_status_updated = pyqtSignal(int, object)  # ch, proto.DrvStatus
+    position_updated = pyqtSignal(int, int)       # ch, position (microsteps)
+    motion_done = pyqtSignal(int, int)            # ch, final_position
+    stopped = pyqtSignal(int, int)                # ch, final_position
+    fault_occurred = pyqtSignal(int, int)         # ch, fault_mask
+    ack_received = pyqtSignal(int, int)           # req_cmd, status (0=OK)
+    frame_received = pyqtSignal(int, bytes)       # raw (cmd, payload) for log
+
+    def __init__(self, parent=None):
+        super().__init__(parent)
+        self._ser: serial.Serial | None = None
+        self._reader: _T3RReader | None = None
+        self._write_lock = threading.Lock()
+        self._tearing_down = False
+        self._is_open = False
+
+        self._poll_timer = QTimer(self)
+        self._poll_timer.setInterval(250)
+        self._poll_timer.timeout.connect(self._poll)
+
+    # ── Connection ────────────────────────────────────────────────────────────
+
+    @property
+    def is_open(self) -> bool:
+        return self._is_open
+
+    def connect(self, port: str, baud: int = 115200) -> None:
+        """Open the serial port and start the reader. Emits port_opened on success."""
+        if self._is_open:
+            self.disconnect()
+        try:
+            self._ser = serial.Serial(port, baudrate=baud, timeout=0.05)
+        except Exception as exc:
+            raise RuntimeError(f"Cannot open {port}: {exc}") from exc
+
+        self._tearing_down = False
+        self._is_open = True
+        self._reader = _T3RReader(self._ser)
+        self._reader.frame.connect(self._on_frame)
+        self._reader.finished_reason.connect(self._on_reader_finished)
+        self._reader.start()
+        self.port_opened.emit()
+        self.send_frame(proto.ping())   # handshake; polling starts on PONG
+
+    def disconnect(self) -> None:
+        """Close port and stop polling."""
+        self._teardown("")
+
+    def _on_reader_finished(self, reason: str):
+        if reason:
+            self._teardown(reason)
+
+    def _teardown(self, reason: str):
+        if self._tearing_down or not self._is_open:
+            return
+        self._tearing_down = True
+        self._poll_timer.stop()
+        self._is_open = False
+
+        reader, self._reader = self._reader, None
+        ser, self._ser = self._ser, None
+
+        if reader is not None:
+            reader.stop()
+            if QThread.currentThread() is not reader:
+                reader.wait(1000)
+        if ser is not None:
+            try:
+                ser.close()
+            except Exception:
+                pass
+
+        self.disconnected.emit(reason)
+
+    # ── Sending ───────────────────────────────────────────────────────────────
+
+    def send_frame(self, frame: bytes) -> bool:
+        if not self._is_open or self._ser is None:
+            return False
+        try:
+            with self._write_lock:
+                self._ser.write(frame)
+            return True
+        except Exception as exc:
+            self._teardown(str(exc) or exc.__class__.__name__)
+            return False
+
+    # ── Command API ───────────────────────────────────────────────────────────
+
+    def ping(self):
+        self.send_frame(proto.ping())
+
+    def stop_all(self):
+        self.send_frame(proto.stop_all())
+
+    def enable(self, ch: int):
+        self.send_frame(proto.enable(ch))
+
+    def disable(self, ch: int):
+        self.send_frame(proto.disable(ch))
+
+    def enable_mask(self, mask: int):
+        self.send_frame(proto.enable_mask(mask))
+
+    def set_microstep(self, ch: int, microsteps: int):
+        self.send_frame(proto.set_microstep(ch, microsteps))
+
+    def set_current(self, ch: int, run_ma: int, hold_ma: int, ihold_delay: int):
+        self.send_frame(proto.set_current(ch, run_ma, hold_ma, ihold_delay))
+
+    def move(self, ch: int, steps: int, velocity: int, accel: int):
+        self.send_frame(proto.move(ch, steps, velocity, accel))
+
+    def jog(self, ch: int, velocity: int, accel: int):
+        self.send_frame(proto.jog(ch, velocity, accel))
+
+    def stop(self, ch: int, hard: bool = False):
+        self.send_frame(proto.stop(ch, hard))
+
+    def move_group(self, mask: int, steps: int, velocity: int, accel: int):
+        self.send_frame(proto.move_group(mask, steps, velocity, accel))
+
+    def jog_group(self, mask: int, velocity: int, accel: int):
+        self.send_frame(proto.jog_group(mask, velocity, accel))
+
+    def get_info(self, ch: int):
+        self.send_frame(proto.get_info(ch))
+
+    def get_drv_status(self, ch: int):
+        self.send_frame(proto.get_drv_status(ch))
+
+    def get_position(self, ch: int):
+        self.send_frame(proto.get_position(ch))
+
+    def set_position(self, ch: int, position: int):
+        self.send_frame(proto.set_position(ch, position))
+
+    # ── Rotation helpers ──────────────────────────────────────────────────────
+
+    def steps_for_angle(self, angle_deg: float, microsteps: int) -> int:
+        """Compute GR-axis microsteps needed to rotate the stage by angle_deg."""
+        gear_ratio = self.GEAR_TEETH_STAGE / self.GEAR_TEETH_MOTOR
+        steps_per_stage_rev = self.MOTOR_FULL_STEPS_PER_REV * microsteps * gear_ratio
+        return round(steps_per_stage_rev * angle_deg / 360.0)
+
+    def rotate_stage(self, angle_deg: float, microsteps: int,
+                     velocity: int = 8000, accel: int = 4000):
+        """Move GR-axis by the number of steps that rotate the stage by angle_deg."""
+        steps = self.steps_for_angle(angle_deg, microsteps)
+        self.move(self.GR_AXIS_CH, steps, velocity, accel)
+
+    # ── Polling ───────────────────────────────────────────────────────────────
+
+    def start_polling(self):
+        self._poll_timer.start()
+
+    def stop_polling(self):
+        self._poll_timer.stop()
+
+    def _poll(self):
+        if not self._is_open:
+            return
+        for ch in range(proto.NUM_CHANNELS):
+            self.send_frame(proto.get_info(ch))
+
+    # ── Frame dispatcher ──────────────────────────────────────────────────────
+
+    def _on_frame(self, cmd: int, payload: bytes):
+        self.frame_received.emit(cmd, payload)
+
+        if cmd == proto.RSP_PONG:
+            p = proto.decode_pong(payload)
+            if p:
+                self.handshake_ok.emit(p.proto_ver, p.fw_version, p.num_channels)
+                self.start_polling()
+
+        elif cmd == proto.RSP_ACK:
+            a = proto.decode_ack(payload)
+            if a:
+                self.ack_received.emit(a.req_cmd, a.status)
+
+        elif cmd == proto.RSP_INFO:
+            info = proto.decode_info(payload)
+            if info and 0 <= info.ch < proto.NUM_CHANNELS:
+                self.info_updated.emit(info.ch, info)
+
+        elif cmd == proto.RSP_DRV_STATUS:
+            st = proto.decode_drv_status(payload)
+            if st and 0 <= st.ch < proto.NUM_CHANNELS:
+                self.drv_status_updated.emit(st.ch, st)
+
+        elif cmd == proto.RSP_POSITION:
+            pos = proto.decode_position(payload)
+            if pos and 0 <= pos.ch < proto.NUM_CHANNELS:
+                self.position_updated.emit(pos.ch, pos.position)
+
+        elif cmd == proto.EVT_MOTION_DONE:
+            ev = proto.decode_event_position(payload)
+            if ev and 0 <= ev.ch < proto.NUM_CHANNELS:
+                self.motion_done.emit(ev.ch, ev.position)
+
+        elif cmd == proto.EVT_STOPPED:
+            ev = proto.decode_event_position(payload)
+            if ev and 0 <= ev.ch < proto.NUM_CHANNELS:
+                self.stopped.emit(ev.ch, ev.position)
+
+        elif cmd == proto.EVT_FAULT:
+            ev = proto.decode_fault(payload)
+            if ev and 0 <= ev.ch < proto.NUM_CHANNELS:
+                self.fault_occurred.emit(ev.ch, ev.position)