""" motor_driver.py — XorTech 2-Motor USB CDC Controller Driver ============================================================ A clean Python class for controlling the XorTech dual motor controller over USB CDC. No GUI dependencies — suitable for embedding in any Python project, script, or robot controller. Requires: pip install pyserial Quick start: from motor_driver import MotorController, MotorConfig mc = MotorController() mc.connect() # auto-detect by VID/PID mc.configure(1, MotorConfig(ppr=14, gear_ratio=50.0, kp=0.065, ki=0.08, encoder_inverted=True)) mc.set_direction(1, "CW") mc.set_pid_target(1, rpm=3000) mc.enable_pid(1) rpm = mc.get_rpm(1) mc.wait_for_rpm(1, target_rpm=3000, tolerance=50, timeout=10.0) mc.stop() Packet protocol (matches firmware): Command (host→FW): 0x55, 11 bytes Config (host→FW): 0xBB, 22 bytes RPM (FW→host): 0xAA, 14 bytes PID debug(FW→host): 0xBB, 27 bytes """ import serial import serial.tools.list_ports import threading import struct import time from dataclasses import dataclass, field from typing import Optional, Dict, Callable # ---------------------------------------------------------------------- # Exceptions # ---------------------------------------------------------------------- class MotorControllerError(Exception): """Base exception for all MotorController errors.""" pass class NotConnectedError(MotorControllerError): """Raised when an operation requires a connected port.""" pass class TimeoutError(MotorControllerError): """Raised when wait_for_rpm exceeds the timeout.""" pass class InvalidMotorError(MotorControllerError): """Raised when an invalid motor ID is specified.""" pass # ---------------------------------------------------------------------- # Configuration dataclass # ---------------------------------------------------------------------- @dataclass class MotorConfig: """Per-motor configuration sent to firmware via config packet. All fields match the firmware MotorConfig_t struct. Pass an instance to MotorController.configure() for each motor before use. """ ppr: int = 14 # Encoder lines per motor shaft rev (datasheet) gear_ratio: float = 50.0 # Motor shaft revs per output shaft rev wheel_diameter_mm: float = 0.0 # Output shaft wheel diameter (0 = no wheel) kp: float = 0.05 # PID proportional gain ki: float = 0.02 # PID integral gain kd: float = 0.0 # PID derivative gain integral_limit: float = 10000.0 # Anti-windup clamp max_pwm_step: int = 10 # Max PWM change per 100ms window encoder_inverted: bool = False # Invert encoder direction debug: bool = False # Enable PID debug packets from firmware # ---------------------------------------------------------------------- # Internal motor state # ---------------------------------------------------------------------- @dataclass class _MotorState: """Internal state for one motor.""" direction: str = "STOP" # "CW", "CCW", "STOP", "BRAKE" pwm: int = 0 # 0-255 pid_enabled: bool = False pid_target: float = 0.0 # motor shaft RPM config: MotorConfig = field(default_factory=MotorConfig) @property def in1(self) -> int: return {"CW": 1, "CCW": 0, "STOP": 0, "BRAKE": 1}.get(self.direction, 0) @property def in2(self) -> int: return {"CW": 0, "CCW": 1, "STOP": 0, "BRAKE": 1}.get(self.direction, 0) # ---------------------------------------------------------------------- # Background serial reader # ---------------------------------------------------------------------- class _SerialReader(threading.Thread): """Background thread that reads RPM and debug packets from firmware.""" ENCODER_HEADER = 0xAA ENCODER_PKT_SIZE = 14 # 0xAA + M1_rpm(4) + M2_rpm(4) + sync_err(4) + sync_on(1) DEBUG_HEADER = 0xBB DEBUG_PKT_SIZE = 27 # 0xBB + motor_id(1) + 6×float(24) + pwm(1) def __init__(self, port, rpm_callback, debug_callback): super().__init__(daemon=True) self._port = port self._rpm_callback = rpm_callback self._debug_callback = debug_callback self._stop_event = threading.Event() self._buffer = bytearray() def stop(self): self._stop_event.set() def run(self): while not self._stop_event.is_set(): try: if not (self._port and self._port.is_open): time.sleep(0.05) continue waiting = self._port.in_waiting chunk = self._port.read(waiting if waiting else 1) if chunk: self._buffer.extend(chunk) self._parse() else: time.sleep(0.005) except serial.SerialException: break except Exception: self._buffer.clear() time.sleep(0.01) def _parse(self): while True: if not self._buffer: return # Find nearest known header aa = self._buffer.find(bytes([self.ENCODER_HEADER])) bb = self._buffer.find(bytes([self.DEBUG_HEADER])) if aa == -1 and bb == -1: self._buffer.clear() return if aa == -1: next_idx = bb elif bb == -1: next_idx = aa else: next_idx = min(aa, bb) if next_idx > 0: del self._buffer[:next_idx] header = self._buffer[0] if header == self.ENCODER_HEADER: if len(self._buffer) < self.ENCODER_PKT_SIZE: return pkt = bytes(self._buffer[:self.ENCODER_PKT_SIZE]) del self._buffer[:self.ENCODER_PKT_SIZE] try: rpm1, rpm2, sync_err = struct.unpack('<3l', pkt[1:13]) sync_on = pkt[13] != 0 self._rpm_callback(rpm1 / 10.0, rpm2 / 10.0, sync_err, sync_on) except struct.error: pass elif header == self.DEBUG_HEADER: if len(self._buffer) < self.DEBUG_PKT_SIZE: return pkt = bytes(self._buffer[:self.DEBUG_PKT_SIZE]) del self._buffer[:self.DEBUG_PKT_SIZE] try: motor_id = pkt[1] sp, meas, err, p, i, d = struct.unpack('<6f', pkt[2:26]) pwm = pkt[26] self._debug_callback(motor_id, sp, meas, err, p, i, d, pwm) except struct.error: pass else: del self._buffer[:1] # ---------------------------------------------------------------------- # Main MotorController class # ---------------------------------------------------------------------- class MotorController: """ Driver for the XorTech 2-Motor USB CDC Controller. Manages connection, configuration, motor control commands and RPM readback. Thread-safe — RPM values are updated by a background reader thread and can be read from any thread. Example: mc = MotorController() mc.connect() mc.configure(1, MotorConfig(ppr=14, gear_ratio=50, kp=0.065, ki=0.08)) mc.configure(2, MotorConfig(ppr=14, gear_ratio=50, kp=0.065, ki=0.08)) mc.set_direction(1, "CW") mc.set_pid_target(1, 3000) mc.enable_pid(1) mc.wait_for_rpm(1, 3000, tolerance=100, timeout=10) print(mc.get_rpm(1)) mc.stop() mc.disconnect() Context manager: with MotorController() as mc: mc.connect() mc.configure(1, MotorConfig(...)) mc.set_direction(1, "CW") mc.enable_pid(1) mc.set_pid_target(1, 3000) time.sleep(5) # motors automatically stopped on exit """ KNOWN_VID_PID = [(0x04D8, 0x0B15)] NUM_MOTORS = 2 # Command packet flags _FLAG_M1_PID = (1 << 0) _FLAG_M2_PID = (1 << 1) _FLAG_SYNC = (1 << 2) # Config packet flags _CFG_FLAG_DEBUG = (1 << 0) _CFG_FLAG_INVERTED = (1 << 1) def __init__(self): self._port: Optional[serial.Serial] = None self._reader: Optional[_SerialReader] = None self._lock = threading.Lock() # Latest RPM from firmware (motor shaft RPM, signed) self._rpm: Dict[int, float] = {1: 0.0, 2: 0.0} # Sync state self._sync_err = 0 self._sync_on = False # Per-motor state self._state: Dict[int, _MotorState] = { 1: _MotorState(), 2: _MotorState(), } # Optional debug callback: fn(motor_id, setpoint, measured, # error, p_term, i_term, d_term, pwm) self.on_debug: Optional[Callable] = None # Optional RPM callback: fn(motor1_rpm, motor2_rpm) self.on_rpm: Optional[Callable] = None # ------------------------------------------------------------------ # Context manager # ------------------------------------------------------------------ def __enter__(self): return self def __exit__(self, *_): try: self.stop() except Exception: pass self.disconnect() # ------------------------------------------------------------------ # Connection # ------------------------------------------------------------------ def connect(self, port: Optional[str] = None, baudrate: int = 115200) -> str: """Connect to the motor controller. Args: port: Serial port name (e.g. "COM3", "/dev/ttyACM0"). If None, auto-detects by VID/PID. baudrate: Baud rate (default 115200). Returns: The port name that was connected. Raises: MotorControllerError: If no device found or connection fails. """ if port is None: port = self._auto_detect() if port is None: raise MotorControllerError( "No XorTech motor controller found. " "Check USB connection or specify port manually.") try: self._port = serial.Serial(port, baudrate=baudrate, timeout=0) except serial.SerialException as e: raise MotorControllerError(f"Failed to open {port}: {e}") from e self._reader = _SerialReader( self._port, self._on_rpm_packet, self._on_debug_packet) self._reader.start() # Send stop packet to reset firmware state self._send_stop() return port def disconnect(self): """Stop reader thread and close serial port.""" if self._reader: self._reader.stop() self._reader.join(timeout=1) self._reader = None if self._port and self._port.is_open: self._port.close() self._port = None @property def connected(self) -> bool: """True if serial port is open.""" return bool(self._port and self._port.is_open) def _auto_detect(self) -> Optional[str]: """Find the first port matching a known VID/PID.""" for port in serial.tools.list_ports.comports(): if (port.vid, port.pid) in self.KNOWN_VID_PID: return port.device return None # ------------------------------------------------------------------ # Configuration # ------------------------------------------------------------------ def configure(self, motor: int, config: MotorConfig): """Send motor configuration to firmware. Must be called before enabling PID. Safe to call while motor is running — gains update immediately. Args: motor: Motor ID (1 or 2). config: MotorConfig instance with all parameters. """ self._check_motor(motor) self._check_connected() self._state[motor].config = config self._send_config(motor) # ------------------------------------------------------------------ # Motor control # ------------------------------------------------------------------ def set_direction(self, motor: int, direction: str): """Set motor direction. Args: motor: Motor ID (1 or 2). direction: "CW", "CCW", "STOP", or "BRAKE". """ self._check_motor(motor) direction = direction.upper() if direction not in ("CW", "CCW", "STOP", "BRAKE"): raise MotorControllerError( f"Invalid direction '{direction}'. Use CW, CCW, STOP or BRAKE.") self._state[motor].direction = direction self._send_command() def set_pwm(self, motor: int, pwm: int): """Set manual PWM output (0-255). Only active when PID is disabled. Args: motor: Motor ID (1 or 2). pwm: PWM value 0-255. """ self._check_motor(motor) self._state[motor].pwm = max(0, min(255, int(pwm))) self._send_command() def set_pid_target(self, motor: int, rpm: float): """Set PID setpoint in motor shaft RPM. Args: motor: Motor ID (1 or 2). rpm: Target motor shaft RPM. """ self._check_motor(motor) self._state[motor].pid_target = max(0.0, float(rpm)) self._send_command() def set_pid_target_output_rpm(self, motor: int, output_rpm: float): """Set PID setpoint in output shaft RPM (converted via gear ratio). Args: motor: Motor ID (1 or 2). output_rpm: Target output shaft RPM. """ self._check_motor(motor) gear = self._state[motor].config.gear_ratio self.set_pid_target(motor, output_rpm * gear) def set_pid_target_velocity(self, motor: int, velocity_ms: float): """Set PID setpoint in m/s (requires wheel_diameter_mm in config). Args: motor: Motor ID (1 or 2). velocity_ms: Target velocity in m/s. """ import math self._check_motor(motor) cfg = self._state[motor].config if cfg.wheel_diameter_mm <= 0: raise MotorControllerError( "wheel_diameter_mm must be set in MotorConfig for velocity control.") circ = math.pi * cfg.wheel_diameter_mm / 1000.0 output_rpm = velocity_ms / circ * 60.0 self.set_pid_target_output_rpm(motor, output_rpm) def enable_pid(self, motor: int): """Enable PID control for a motor. Args: motor: Motor ID (1 or 2), or None for both. """ self._check_motor(motor) self._state[motor].pid_enabled = True self._send_command() def disable_pid(self, motor: int): """Disable PID control for a motor (returns to manual PWM). Args: motor: Motor ID (1 or 2). """ self._check_motor(motor) self._state[motor].pid_enabled = False self._send_command() def stop(self, motor: Optional[int] = None): """Stop one or both motors (coast — IN1=IN2=0). Args: motor: Motor ID (1 or 2), or None to stop both. """ if motor is None: for m in range(1, self.NUM_MOTORS + 1): self._state[m].direction = "STOP" self._state[m].pwm = 0 self._state[m].pid_enabled = False self._state[m].pid_target = 0.0 else: self._check_motor(motor) self._state[motor].direction = "STOP" self._state[motor].pwm = 0 self._state[motor].pid_enabled = False self._state[motor].pid_target = 0.0 self._send_command() def brake(self, motor: Optional[int] = None): """Brake one or both motors (IN1=IN2=1). Args: motor: Motor ID (1 or 2), or None to brake both. """ if motor is None: for m in range(1, self.NUM_MOTORS + 1): self._state[m].direction = "BRAKE" self._state[m].pid_enabled = False else: self._check_motor(motor) self._state[motor].direction = "BRAKE" self._state[motor].pid_enabled = False self._send_command() # ------------------------------------------------------------------ # Sync # ------------------------------------------------------------------ def enable_sync(self): """Enable encoder count synchronisation (Motor 2 tracks Motor 1). Both motors must be stopped and PIDs disabled before enabling sync. """ self._check_connected() self._sync_on = True self._send_command() def disable_sync(self): """Disable encoder count synchronisation.""" self._check_connected() self._sync_on = False self._send_command() @property def sync_error(self) -> int: """Current encoder count difference between motors (M1 - M2).""" with self._lock: return self._sync_err # ------------------------------------------------------------------ # RPM readback # ------------------------------------------------------------------ def get_rpm(self, motor: Optional[int] = None): """Get latest motor shaft RPM from firmware. Args: motor: Motor ID (1 or 2), or None for both motors. Returns: float if motor specified, dict {1: float, 2: float} if None. """ with self._lock: if motor is None: return {1: self._rpm[1], 2: self._rpm[2]} self._check_motor(motor) return self._rpm[motor] def get_output_rpm(self, motor: int) -> float: """Get output shaft RPM (motor shaft RPM ÷ gear ratio). Args: motor: Motor ID (1 or 2). Returns: Output shaft RPM as float. """ self._check_motor(motor) gear = self._state[motor].config.gear_ratio return abs(self.get_rpm(motor)) / gear if gear > 0 else 0.0 def get_velocity(self, motor: int) -> float: """Get velocity in m/s (requires wheel_diameter_mm in config). Args: motor: Motor ID (1 or 2). Returns: Velocity in m/s. """ import math self._check_motor(motor) cfg = self._state[motor].config if cfg.wheel_diameter_mm <= 0: raise MotorControllerError( "wheel_diameter_mm must be set in MotorConfig for velocity readback.") circ = math.pi * cfg.wheel_diameter_mm / 1000.0 return self.get_output_rpm(motor) / 60.0 * circ def wait_for_rpm(self, motor: int, target_rpm: float, tolerance: float = 50.0, timeout: float = 10.0) -> float: """Block until motor shaft RPM is within tolerance of target. Args: motor: Motor ID (1 or 2). target_rpm: Target motor shaft RPM. tolerance: Acceptable RPM error (default ±50 RPM). timeout: Maximum wait time in seconds (default 10s). Returns: Actual RPM when settled. Raises: TimeoutError: If RPM doesn't settle within timeout. """ self._check_motor(motor) deadline = time.monotonic() + timeout while time.monotonic() < deadline: rpm = abs(self.get_rpm(motor)) if abs(rpm - target_rpm) <= tolerance: return rpm time.sleep(0.1) raise TimeoutError( f"Motor {motor} did not reach {target_rpm:.0f} RPM " f"within {timeout:.1f}s (last: {abs(self.get_rpm(motor)):.0f} RPM)") def wait_for_velocity(self, motor: int, target_ms: float, tolerance: float = 0.01, timeout: float = 10.0) -> float: """Block until velocity is within tolerance of target (m/s). Args: motor: Motor ID (1 or 2). target_ms: Target velocity in m/s. tolerance: Acceptable velocity error in m/s (default ±0.01 m/s). timeout: Maximum wait time in seconds (default 10s). Returns: Actual velocity when settled. Raises: TimeoutError: If velocity doesn't settle within timeout. """ self._check_motor(motor) deadline = time.monotonic() + timeout while time.monotonic() < deadline: vel = self.get_velocity(motor) if abs(vel - target_ms) <= tolerance: return vel time.sleep(0.1) raise TimeoutError( f"Motor {motor} did not reach {target_ms:.3f} m/s " f"within {timeout:.1f}s (last: {self.get_velocity(motor):.3f} m/s)") def stream_rpm(self, duration: float, interval: float = 0.1): """Generator that yields (timestamp, rpm1, rpm2) at regular intervals. Args: duration: Total streaming duration in seconds. interval: Yield interval in seconds (default 0.1s = 10Hz). Yields: Tuple of (elapsed_seconds, motor1_rpm, motor2_rpm). """ start = time.monotonic() while True: elapsed = time.monotonic() - start if elapsed > duration: break rpms = self.get_rpm() yield elapsed, rpms[1], rpms[2] time.sleep(interval) # ------------------------------------------------------------------ # Packet building and sending # ------------------------------------------------------------------ def _send_command(self): """Build and send 11-byte command packet.""" if not self.connected: return cdcdata = bytearray(11) cdcdata[0] = 0x55 cdcdata[1] = 0x03 # M01_STBY enable for m in (1, 2): s = self._state[m] dir_byte = (s.in1 * 1) + (s.in2 * 2) cdcdata[m * 2] = dir_byte cdcdata[m * 2 + 1] = s.pwm # Control flags byte 6 ctrl = 0 if self._state[1].pid_enabled: ctrl |= self._FLAG_M1_PID if self._state[2].pid_enabled: ctrl |= self._FLAG_M2_PID if self._sync_on: ctrl |= self._FLAG_SYNC cdcdata[6] = ctrl # Setpoints — bytes 7-10 for m in (1, 2): sp_x10 = int(max(0.0, min(self._state[m].pid_target * 10.0, 65535.0))) base = 5 + m * 2 # M1→7,8 M2→9,10 cdcdata[base] = (sp_x10 >> 8) & 0xFF cdcdata[base + 1] = sp_x10 & 0xFF try: self._port.write(bytes(cdcdata)) except serial.SerialException as e: raise MotorControllerError(f"Failed to send command: {e}") from e def _send_config(self, motor: int): """Build and send 22-byte config packet for one motor.""" cfg = self._state[motor].config packet = bytearray(22) packet[0] = 0xBB packet[1] = motor struct.pack_into('"