haptic_skin.transport

  1"""Transports that carry binary protocol frames to the controller.
  2
  3Two implementations:
  4  * ``SerialTransport`` — talks to a real Pico over USB CDC-ACM (pyserial).
  5  * ``MockTransport``   — an in-process simulator (``FakeMega``) so the whole
  6    stack can be developed and tested with no hardware attached.
  7
  8Both expose the same ``send(frame: bytes) -> bytes`` API: a command frame in,
  9a reply frame out (empty ``b""`` when the device has nothing to say).
 10"""
 11
 12from __future__ import annotations
 13
 14from abc import ABC, abstractmethod
 15from typing import List, Optional
 16
 17from . import protocol
 18
 19# Power-guard / safety constants — mirror the firmware (firmware-rust-v2) so the
 20# simulator behaves exactly like the real board:
 21#   * MAX_DUTY        = config::MOTOR_MAX_DUTY  (Motors::set clamps EVERY write)
 22#   * DUTY_CAP        = config::POWER_GUARD_DUTY_CAP (SetAll only)
 23#   * MAX_ACTIVE_MOTORS = config::POWER_GUARD_MAX_CONCURRENT (SetAll only)
 24MAX_DUTY = 150
 25DUTY_CAP = 153
 26MAX_ACTIVE_MOTORS = 6
 27
 28FW_VERSION = "1.0"
 29
 30
 31class Transport(ABC):
 32    """Sends one protocol frame and returns the controller's reply frame."""
 33
 34    @abstractmethod
 35    def send(self, frame: bytes) -> bytes:  # pragma: no cover - interface
 36        ...
 37
 38    def close(self) -> None:  # pragma: no cover - default no-op
 39        pass
 40
 41    def __enter__(self) -> "Transport":
 42        return self
 43
 44    def __exit__(self, *exc) -> None:
 45        self.close()
 46
 47
 48class FakeMega:
 49    """Pure-Python re-implementation of the firmware command handler.
 50
 51    Decodes binary frames, mirrors the power guard, and tracks motor state so
 52    tests and offline development see the same behaviour as the hardware.
 53    (Name kept for continuity; it now emulates the Pico/Rust firmware.)
 54    """
 55
 56    def __init__(self) -> None:
 57        self.motors: List[int] = [0] * protocol.NUM_MOTORS
 58        # history of every committed vector — handy for assertions in tests
 59        self.history: List[List[int]] = []
 60
 61    @staticmethod
 62    def _clamp(value: int) -> int:
 63        """Mirror ``Motors::set`` — every motor write is bounded to MAX_DUTY."""
 64        return max(0, min(MAX_DUTY, value))
 65
 66    @staticmethod
 67    def _power_guard(values: List[int]) -> List[int]:
 68        """Mirror ``tasks::power_guard::apply`` (the SetAll path only).
 69
 70        1) cap each motor to ``DUTY_CAP``; 2) keep the ``MAX_ACTIVE_MOTORS``
 71        strongest non-zero motors, cut the rest to 0.
 72        """
 73        v = [min(DUTY_CAP, max(0, x)) for x in values]
 74        ranked = sorted((i for i, x in enumerate(v) if x > 0),
 75                        key=lambda i: v[i], reverse=True)
 76        for i in ranked[MAX_ACTIVE_MOTORS:]:
 77            v[i] = 0
 78        return v
 79
 80    def _commit(self, values: List[int]) -> None:
 81        self.motors = [self._clamp(v) for v in values]
 82        self.history.append(list(self.motors))
 83
 84    def handle(self, frame: bytes) -> bytes:
 85        """Process one host->device frame, return the reply frame (or ``b""``)."""
 86        decoded = protocol.decode_one(frame, start_byte=protocol.START_H2P)
 87        if decoded is None:
 88            return protocol.error()
 89        cmd, payload = decoded
 90
 91        if cmd == protocol.CMD_PING:
 92            return protocol.pong()
 93
 94        if cmd == protocol.CMD_STOP_ALL:
 95            self._commit([0] * protocol.NUM_MOTORS)
 96            return b""
 97
 98        if cmd == protocol.CMD_SET_MOTOR:
 99            # firmware SetMotor path: direct write, clamped, NO concurrency guard
100            if len(payload) != 2 or not 0 <= payload[0] < protocol.NUM_MOTORS:
101                return protocol.error()
102            req = list(self.motors)
103            req[payload[0]] = payload[1]
104            self._commit(req)
105            return b""
106
107        if cmd == protocol.CMD_SET_ALL:
108            # firmware SetAll path: power guard (cap + concurrency) then clamp
109            if len(payload) != protocol.NUM_MOTORS:
110                return protocol.error()
111            self._commit(self._power_guard(list(payload)))
112            return b""
113
114        return protocol.error()
115
116
117class MockTransport(Transport):
118    """In-process transport backed by :class:`FakeMega` (no hardware)."""
119
120    def __init__(self) -> None:
121        self.device = FakeMega()
122        self.sent: List[bytes] = []
123
124    def send(self, frame: bytes) -> bytes:
125        self.sent.append(frame)
126        return self.device.handle(frame)
127
128
129def _is_ping(frame: bytes) -> bool:
130    return len(frame) >= 2 and frame[0] == protocol.START_H2P and frame[1] == protocol.CMD_PING
131
132
133class SerialTransport(Transport):
134    """Talks to a real Pico over USB CDC-ACM — resilient to unplug/replug.
135
136    Never raises if the board is missing: it keeps trying to (re)open in the
137    background so the bridge stays up across a disconnect, and reconnects on its
138    own when the Pico comes back. Only ``Ping`` frames wait for a reply; every
139    other frame is fire-and-forget (so streaming motor updates never block).
140    """
141
142    def __init__(self, port: str, baud: int = 115200, timeout: float = 0.5):
143        try:
144            import serial  # type: ignore
145        except ImportError as exc:  # pragma: no cover
146            raise RuntimeError(
147                "pyserial is required for SerialTransport — `pip install pyserial`"
148            ) from exc
149        self._serial_mod = serial
150        self.port, self.baud, self.timeout = port, baud, timeout
151        self._serial = None
152        self._last_try = 0.0
153        self._open(initial=True)
154
155    def _open(self, initial: bool = False) -> None:
156        import time
157        try:
158            self._serial = self._serial_mod.Serial(self.port, self.baud, timeout=self.timeout)
159            if initial:
160                time.sleep(2.0)            # let the board enumerate / settle
161            self._serial.reset_input_buffer()
162            print(f"[serial] connected to {self.port}")
163        except Exception:
164            self._serial = None
165            if initial:
166                print(f"[serial] {self.port} not available yet — will retry")
167
168    def _ensure(self) -> bool:
169        if self._serial is not None:
170            return True
171        import time
172        now = time.monotonic()
173        if now - self._last_try < 2.0:     # throttle reconnect attempts
174            return False
175        self._last_try = now
176        self._open()
177        return self._serial is not None
178
179    def send(self, frame: bytes) -> bytes:
180        if not self._ensure():
181            return b""                      # board absent: no-op, keep running
182        try:
183            self._serial.write(frame)
184            self._serial.flush()
185            if _is_ping(frame):
186                return self._serial.read(64)  # pong frame (or b"" on timeout)
187            return b""                       # set commands are fire-and-forget
188        except Exception:                    # unplugged mid-session
189            try:
190                self._serial.close()
191            except Exception:
192                pass
193            self._serial = None
194            print(f"[serial] lost {self.port} — will reconnect")
195            return b""
196
197    def close(self) -> None:
198        try:
199            if self._serial is not None:
200                self._serial.close()
201        except Exception:  # pragma: no cover
202            pass
203
204
205def open_transport(port: Optional[str] = None) -> Transport:
206    """Open a real serial transport if ``port`` is given, else a mock."""
207    if port:
208        return SerialTransport(port)
209    return MockTransport()