haptic_skin.protocol

  1"""Binary serial protocol shared by the host and the Pico firmware.
  2
  3Frames (see ``docs/protocol.md`` and ``firmware-rust-v2/src/proto.rs``)::
  4
  5    host -> Pico : 0xAA [cmd]   [len] [payload…] [xor]
  6    Pico -> host : 0xBB [event] [len] [payload…] [xor]
  7
  8``xor`` is the XOR of ``[type, len, *payload]`` (everything but the start byte
  9and the xor itself). This module only builds/parses frames and clamps values;
 10it does not touch any transport.
 11"""
 12
 13from __future__ import annotations
 14
 15from typing import Iterator, Optional, Sequence, Tuple
 16
 17NUM_MOTORS = 8
 18PWM_MAX = 255
 19
 20START_H2P = 0xAA
 21START_P2H = 0xBB
 22
 23# Commands (host -> Pico)
 24CMD_PING = 0x01
 25CMD_SET_MOTOR = 0x10
 26CMD_SET_ALL = 0x11
 27CMD_STOP_ALL = 0x12
 28
 29# Events (Pico -> host)
 30EVT_PONG = 0x01
 31EVT_ERROR = 0xFF
 32
 33# Identifier returned by Necklace.version() (no on-wire version command).
 34FW_ID = "HAPTIC.SKIN-PICO"
 35
 36
 37def clamp_pwm(value: int) -> int:
 38    """Clamp an integer to the valid PWM range 0..255."""
 39    return max(0, min(PWM_MAX, int(value)))
 40
 41
 42def intensity_to_pwm(intensity: float) -> int:
 43    """Map a 0.0..1.0 intensity to a 0..255 PWM value."""
 44    return clamp_pwm(round(float(intensity) * PWM_MAX))
 45
 46
 47def _xor(data: bytes) -> int:
 48    x = 0
 49    for b in data:
 50        x ^= b
 51    return x
 52
 53
 54def _frame(start: int, type_: int, payload: bytes = b"") -> bytes:
 55    body = bytes([type_, len(payload)]) + payload
 56    return bytes([start]) + body + bytes([_xor(body)])
 57
 58
 59# -- host -> Pico builders --------------------------------------------------
 60def ping() -> bytes:
 61    """``Ping`` — the Pico replies with ``Pong``."""
 62    return _frame(START_H2P, CMD_PING)
 63
 64
 65def stop() -> bytes:
 66    """``StopAll`` — cut every motor."""
 67    return _frame(START_H2P, CMD_STOP_ALL)
 68
 69
 70def set_motor(index: int, value: int) -> bytes:
 71    """``SetMotor`` — set a single motor (0..7) to a PWM value (0..255)."""
 72    if not 0 <= index < NUM_MOTORS:
 73        raise ValueError(f"motor index out of range: {index}")
 74    return _frame(START_H2P, CMD_SET_MOTOR, bytes([index, clamp_pwm(value)]))
 75
 76
 77def set_all(value: int) -> bytes:
 78    """``SetAll`` with the same value on the 8 motors."""
 79    return _frame(START_H2P, CMD_SET_ALL, bytes([clamp_pwm(value)] * NUM_MOTORS))
 80
 81
 82def set_vector(values: Sequence[int]) -> bytes:
 83    """``SetAll`` from an explicit 8-value vector (used by patterns)."""
 84    if len(values) != NUM_MOTORS:
 85        raise ValueError(f"expected {NUM_MOTORS} values, got {len(values)}")
 86    return _frame(START_H2P, CMD_SET_ALL, bytes(clamp_pwm(v) for v in values))
 87
 88
 89# -- Pico -> host builders (used by the simulator) --------------------------
 90def pong() -> bytes:
 91    return _frame(START_P2H, EVT_PONG)
 92
 93
 94def error(code: int = 0) -> bytes:
 95    return _frame(START_P2H, EVT_ERROR, bytes([code & 0xFF]))
 96
 97
 98class Decoder:
 99    """Incremental byte decoder — mirror of the firmware ``FrameParser``.
100
101    Feed bytes one at a time with :meth:`push`; it returns ``(type, payload)``
102    when a full, XOR-valid frame arrives, else ``None``. Bad frames are dropped
103    and it resynchronises on the next start byte. ``start_byte`` selects the
104    direction (``START_P2H`` on the host, ``START_H2P`` in the simulator).
105    """
106
107    _WAIT_START, _WAIT_TYPE, _WAIT_LEN, _WAIT_PAYLOAD, _WAIT_XOR = range(5)
108
109    def __init__(self, start_byte: int = START_P2H) -> None:
110        self._start = start_byte
111        self._reset()
112
113    def _reset(self) -> None:
114        self._state = self._WAIT_START
115        self._type = 0
116        self._len = 0
117        self._payload = bytearray()
118
119    def push(self, byte: int) -> Optional[Tuple[int, bytes]]:
120        if self._state == self._WAIT_START:
121            if byte == self._start:
122                self._state = self._WAIT_TYPE
123            return None
124        if self._state == self._WAIT_TYPE:
125            self._type = byte
126            self._state = self._WAIT_LEN
127            return None
128        if self._state == self._WAIT_LEN:
129            self._len = byte
130            self._payload = bytearray()
131            self._state = self._WAIT_XOR if byte == 0 else self._WAIT_PAYLOAD
132            return None
133        if self._state == self._WAIT_PAYLOAD:
134            self._payload.append(byte)
135            if len(self._payload) == self._len:
136                self._state = self._WAIT_XOR
137            return None
138        # _WAIT_XOR
139        expected = self._type ^ self._len ^ _xor(bytes(self._payload))
140        out = (self._type, bytes(self._payload)) if byte == expected else None
141        self._reset()
142        return out
143
144    def feed(self, data: bytes) -> Iterator[Tuple[int, bytes]]:
145        """Yield every complete frame found in ``data``."""
146        for b in data:
147            frame = self.push(b)
148            if frame is not None:
149                yield frame
150
151
152def decode_one(data: bytes, start_byte: int = START_P2H) -> Optional[Tuple[int, bytes]]:
153    """Return the first complete frame in ``data`` (or ``None``)."""
154    for frame in Decoder(start_byte).feed(data):
155        return frame
156    return None