tka/scanengine-3
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

precommit

Browse Source
This commit is contained in:
Thomas Ales [MSE]
2026-07-21 20:10:06 -05:00
parent 66e72884fe
commit 1014533626
3 changed files with 1383 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
hardware/t3r_driver.py
Executable
+287
View File
@@ -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)
hardware/t3r_protocol.py
Executable
+374
View File
@@ -0,0 +1,374 @@
"""T3R binary serial protocol — host-side codec.
A faithful Python port of the wire protocol implemented in ``main/protocol.c``.
Pure standard library (no PyQt / pyserial), so it can be unit-tested on its own.
Frame layout (all multi-byte fields little-endian):
+------+------+------+----------------+------+
| SOF | CMD | LEN | PAYLOAD[LEN] | CRC8 |
+------+------+------+----------------+------+
0xA5 1 B 1 B LEN bytes 1 B
CRC8 is CRC-8/SMBUS (poly 0x07, init 0x00) over CMD, LEN and PAYLOAD.
See PROTOCOL.md for the full catalogue.
"""
from __future__ import annotations
import struct
from dataclasses import dataclass
SOF = 0xA5
PROTO_VERSION = 1
# ---- Requests (host -> device) --------------------------------------------
CMD_PING = 0x01
CMD_SET_MICROSTEP = 0x10
CMD_SET_CURRENT = 0x11
CMD_ENABLE = 0x12
CMD_DISABLE = 0x13
CMD_ENABLE_MASK = 0x14
CMD_MOVE = 0x20
CMD_JOG = 0x21
CMD_STOP = 0x22
CMD_STOP_ALL = 0x23
CMD_MOVE_GROUP = 0x24
CMD_JOG_GROUP = 0x25
CMD_GET_INFO = 0x30
CMD_GET_DRV_STATUS = 0x31
CMD_GET_POSITION = 0x32
CMD_SET_POSITION = 0x33
CMD_READ_REG = 0x40
CMD_WRITE_REG = 0x41
# ---- Responses (device -> host) -------------------------------------------
RSP_ACK = 0x81
RSP_PONG = 0x82
RSP_INFO = 0x83
RSP_DRV_STATUS = 0x84
RSP_POSITION = 0x85
RSP_REG = 0x86
# ---- Asynchronous events (device -> host) ---------------------------------
EVT_MOTION_DONE = 0xE0
EVT_STOPPED = 0xE1
EVT_FAULT = 0xE2
# Human-readable names, used by the log/console.
CMD_NAMES = {
CMD_PING: "PING", CMD_SET_MICROSTEP: "SET_MICROSTEP",
CMD_SET_CURRENT: "SET_CURRENT", CMD_ENABLE: "ENABLE", CMD_DISABLE: "DISABLE",
CMD_ENABLE_MASK: "ENABLE_MASK",
CMD_MOVE: "MOVE", CMD_JOG: "JOG", CMD_STOP: "STOP", CMD_STOP_ALL: "STOP_ALL",
CMD_MOVE_GROUP: "MOVE_GROUP", CMD_JOG_GROUP: "JOG_GROUP",
CMD_GET_INFO: "GET_INFO", CMD_GET_DRV_STATUS: "GET_DRV_STATUS",
CMD_GET_POSITION: "GET_POSITION", CMD_SET_POSITION: "SET_POSITION",
CMD_READ_REG: "READ_REG", CMD_WRITE_REG: "WRITE_REG",
RSP_ACK: "ACK", RSP_PONG: "PONG", RSP_INFO: "INFO",
RSP_DRV_STATUS: "DRV_STATUS", RSP_POSITION: "POSITION", RSP_REG: "REG",
EVT_MOTION_DONE: "MOTION_DONE", EVT_STOPPED: "STOPPED", EVT_FAULT: "FAULT",
}
# ---- ACK status codes ------------------------------------------------------
STATUS_NAMES = {
0x00: "OK", 0x02: "bad length", 0x03: "bad channel", 0x04: "bad parameter",
0x05: "busy", 0x06: "SPI comms fault", 0x07: "unknown command",
}
# ---- INFO.state ------------------------------------------------------------
STATE_NAMES = {0: "IDLE", 1: "MOVING", 2: "JOGGING", 3: "STOPPING"}
# ---- Fault mask bits -------------------------------------------------------
FAULT_BITS = [
(1 << 0, "SHORT_GND_A"),
(1 << 1, "SHORT_GND_B"),
(1 << 2, "OVERTEMP"),
(1 << 3, "OVERTEMP_WARN"),
(1 << 4, "OPEN_LOAD_A"),
(1 << 5, "OPEN_LOAD_B"),
]
MICROSTEPS = [1, 2, 4, 8, 16, 32, 64, 128, 256]
MAX_VELOCITY = 200_000 # steps/s (MOTOR_MAX_VELOCITY)
MAX_ACCEL = 2_000_000 # steps/s2 (MOTOR_MAX_ACCEL)
NUM_CHANNELS = 4
def fault_names(mask: int) -> str:
"""Render a fault mask as a comma-separated list, or 'none'."""
names = [name for bit, name in FAULT_BITS if mask & bit]
return ", ".join(names) if names else "none"
# ---------------------------------------------------------------------------
# CRC-8 (poly 0x07, init 0x00) — matches the firmware reference.
# ---------------------------------------------------------------------------
def crc8(data: bytes) -> int:
crc = 0
for byte in data:
crc ^= byte
for _ in range(8):
crc = ((crc << 1) ^ 0x07) & 0xFF if crc & 0x80 else (crc << 1) & 0xFF
return crc
def build_frame(cmd: int, payload: bytes = b"") -> bytes:
body = bytes([cmd, len(payload)]) + payload
return bytes([SOF]) + body + bytes([crc8(body)])
# ---------------------------------------------------------------------------
# Request encoders — each returns a complete frame ready to write.
# ---------------------------------------------------------------------------
def ping() -> bytes:
return build_frame(CMD_PING)
def set_microstep(ch: int, microsteps: int) -> bytes:
return build_frame(CMD_SET_MICROSTEP, struct.pack("<BH", ch, microsteps))
def set_current(ch: int, run_ma: int, hold_ma: int, ihold_delay: int) -> bytes:
return build_frame(CMD_SET_CURRENT,
struct.pack("<BHHB", ch, run_ma, hold_ma, ihold_delay))
def enable(ch: int) -> bytes:
return build_frame(CMD_ENABLE, bytes([ch]))
def disable(ch: int) -> bytes:
return build_frame(CMD_DISABLE, bytes([ch]))
def enable_mask(mask: int) -> bytes:
"""Energise exactly the channels in `mask` (bit i = channel i); 0 = all off."""
return build_frame(CMD_ENABLE_MASK, bytes([mask & 0x0F]))
def move(ch: int, steps: int, velocity: int, accel: int) -> bytes:
return build_frame(CMD_MOVE, struct.pack("<BiII", ch, steps, velocity, accel))
def jog(ch: int, velocity: int, accel: int) -> bytes:
return build_frame(CMD_JOG, struct.pack("<BiI", ch, velocity, accel))
def move_group(mask: int, steps: int, velocity: int, accel: int) -> bytes:
"""Ganged move: every channel in `mask` steps together in lockstep."""
return build_frame(CMD_MOVE_GROUP,
struct.pack("<BiII", mask & 0x0F, steps, velocity, accel))
def jog_group(mask: int, velocity: int, accel: int) -> bytes:
"""Ganged jog: every channel in `mask` runs together (velocity 0 = stop)."""
return build_frame(CMD_JOG_GROUP,
struct.pack("<BiI", mask & 0x0F, velocity, accel))
def stop(ch: int, hard: bool) -> bytes:
return build_frame(CMD_STOP, struct.pack("<BB", ch, 1 if hard else 0))
def stop_all() -> bytes:
return build_frame(CMD_STOP_ALL)
def get_info(ch: int) -> bytes:
return build_frame(CMD_GET_INFO, bytes([ch]))
def get_drv_status(ch: int) -> bytes:
return build_frame(CMD_GET_DRV_STATUS, bytes([ch]))
def get_position(ch: int) -> bytes:
return build_frame(CMD_GET_POSITION, bytes([ch]))
def set_position(ch: int, position: int) -> bytes:
return build_frame(CMD_SET_POSITION, struct.pack("<Bi", ch, position))
def read_reg(ch: int, reg: int) -> bytes:
return build_frame(CMD_READ_REG, struct.pack("<BB", ch, reg))
def write_reg(ch: int, reg: int, value: int) -> bytes:
return build_frame(CMD_WRITE_REG, struct.pack("<BBI", ch, reg, value))
# ---------------------------------------------------------------------------
# Response / event decoders. Each returns a dataclass (or None on bad length).
# ---------------------------------------------------------------------------
@dataclass
class Pong:
proto_ver: int
fw_version: int
num_channels: int
@dataclass
class Ack:
req_cmd: int
status: int
@property
def ok(self) -> bool:
return self.status == 0x00
@dataclass
class Info:
ch: int
state: int
position: int
velocity: int
microsteps: int
run_ma: int
hold_ma: int
enabled: bool
comms_ok: bool
fault_mask: int
@dataclass
class DrvStatus:
ch: int
raw: int
fault_mask: int
@property
def standstill(self) -> bool:
return bool(self.raw & (1 << 31))
@property
def cs_actual(self) -> int:
return (self.raw >> 16) & 0x1F
@property
def sg_result(self) -> int:
return self.raw & 0x3FF
@dataclass
class Position:
ch: int
position: int
@dataclass
class Reg:
ch: int
reg: int
value: int
def decode_pong(p: bytes):
if len(p) < 4:
return None
proto, fw, nch = struct.unpack_from("<BHB", p, 0)
return Pong(proto, fw, nch)
def decode_ack(p: bytes):
if len(p) < 2:
return None
return Ack(p[0], p[1])
def decode_info(p: bytes):
if len(p) < 18:
return None
(ch, state, pos, vel, micro, run_ma, hold_ma, flags, fault) = \
struct.unpack_from("<BBiIHHHBB", p, 0)
return Info(ch, state, pos, vel, micro, run_ma, hold_ma,
bool(flags & 0x01), bool(flags & 0x02), fault)
def decode_drv_status(p: bytes):
if len(p) < 6:
return None
ch, raw, fault = struct.unpack_from("<BIB", p, 0)
return DrvStatus(ch, raw, fault)
def decode_position(p: bytes):
if len(p) < 5:
return None
ch, pos = struct.unpack_from("<Bi", p, 0)
return Position(ch, pos)
def decode_reg(p: bytes):
if len(p) < 6:
return None
ch, reg, value = struct.unpack_from("<BBI", p, 0)
return Reg(ch, reg, value)
def decode_event_position(p: bytes):
"""MOTION_DONE / STOPPED share the (ch, position) layout."""
return decode_position(p)
def decode_fault(p: bytes):
if len(p) < 2:
return None
return Position(p[0], p[1]) # reuse (ch, value); value is the fault mask
# ---------------------------------------------------------------------------
# Incremental frame parser — mirrors the firmware byte-wise state machine.
# ---------------------------------------------------------------------------
class FrameParser:
"""Feed raw bytes, get back a list of (cmd, payload) for each valid frame.
Bad-CRC and oversized frames are dropped silently; the parser resyncs on
the next SOF, exactly like the device-side parser.
"""
MAX_PAYLOAD = 64
_SOF, _CMD, _LEN, _PAYLOAD, _CRC = range(5)
def __init__(self):
self._state = self._SOF
self._cmd = 0
self._len = 0
self._payload = bytearray()
def feed(self, data: bytes):
out = []
for b in data:
if self._state == self._SOF:
if b == SOF:
self._state = self._CMD
elif self._state == self._CMD:
self._cmd = b
self._state = self._LEN
elif self._state == self._LEN:
self._len = b
self._payload.clear()
if b > self.MAX_PAYLOAD:
self._state = self._SOF # too big: resync
elif b == 0:
self._state = self._CRC
else:
self._state = self._PAYLOAD
elif self._state == self._PAYLOAD:
self._payload.append(b)
if len(self._payload) >= self._len:
self._state = self._CRC
elif self._state == self._CRC:
body = bytes([self._cmd, self._len]) + bytes(self._payload)
if crc8(body) == b:
out.append((self._cmd, bytes(self._payload)))
# else bad CRC: drop, resync
self._state = self._SOF
return out