# HansonServo Protocol Migration Plan

## Overview

The firmware has been updated from a simple XOR-checksum protocol to a more robust CRC16 tagged packet protocol. This document describes the changes needed in the desktop software.

---

## Protocol Changes Summary

| Aspect | Old Protocol | New Protocol |
|--------|--------------|--------------|
| Sync bytes | `0xAA 0x55` | `0xA5 0x5A` |
| Checksum | XOR (1 byte) | CRC16-CCITT (2 bytes) |
| Command ID | 1 byte numeric | 4 byte ASCII tag |
| Sequence | None | 2 byte counter |
| Baud rate | 1,000,000 | 1,000,000 (unchanged) |

---

## New Packet Format

```
┌──────┬──────┬─────────┬─────────┬─────────┬───────────┬─────────┐
│ SYNC │ SYNC │   TAG   │ LENGTH  │   SEQ   │  PAYLOAD  │  CRC16  │
│ 0xA5 │ 0x5A │ 4 bytes │ 2 bytes │ 2 bytes │ N bytes   │ 2 bytes │
└──────┴──────┴─────────┴─────────┴─────────┴───────────┴─────────┘
```

### Field Details

| Field | Size | Description |
|-------|------|-------------|
| SYNC0 | 1 | Always `0xA5` |
| SYNC1 | 1 | Always `0x5A` |
| TAG | 4 | ASCII identifier (e.g., "IDNT", "MSET") |
| LENGTH | 2 | Payload length, little-endian |
| SEQ | 2 | Sequence number, little-endian |
| PAYLOAD | N | Command-specific data |
| CRC16 | 2 | CRC16-CCITT over TAG+LENGTH+SEQ+PAYLOAD, little-endian |

### CRC16-CCITT Implementation

```python
def crc16_ccitt(data: bytes, init: int = 0xFFFF) -> int:
    crc = init
    for byte in data:
        crc ^= byte << 8
        for _ in range(8):
            if crc & 0x8000:
                crc = (crc << 1) ^ 0x1021
            else:
                crc <<= 1
            crc &= 0xFFFF
    return crc
```

```csharp
// C# implementation
ushort Crc16Ccitt(byte[] data)
{
    ushort crc = 0xFFFF;
    foreach (byte b in data)
    {
        crc ^= (ushort)(b << 8);
        for (int i = 0; i < 8; i++)
        {
            if ((crc & 0x8000) != 0)
                crc = (ushort)((crc << 1) ^ 0x1021);
            else
                crc <<= 1;
        }
    }
    return crc;
}
```

---

## Command Tag Mapping

### Old → New Command Mapping

| Old Command | Old ID | New Tag | Notes |
|-------------|--------|---------|-------|
| CMD_ID_REQUEST | 0x01 | `IDNT` | Identity request |
| CMD_FILE_LIST | 0x02 | `FLST` | List files |
| CMD_LOAD_FILE | 0x03 | `FLOD` | Load file content |
| CMD_DELETE_FILE | 0x04 | `FDEL` | Delete file |
| CMD_SAVE_FILE | 0x05 | `FSAV` | Save animation |
| CMD_MESSAGE | 0x06 | `MSGE` | Log/debug message |
| CMD_SET_POSITION | 0x07 | `MSET` | Set motor positions |
| CMD_PLAY_FILE | 0x08 | `FPLY` | Play animation |
| CMD_STOP_FILE | 0x09 | `FSTP` | Stop animation |
| CMD_SCAN_CHANNEL | 0x09 | `MSCN` | Scan for motors |
| CMD_WRITE_DATA | 0x10 | `MWRT` | Write motor register |
| CMD_WRITE_CONFIG_UPDATE | 0x12 | `CONF` | Update config |
| CMD_START_POSITION_STREAM | 0x14 | `MSTM` | Motor stream control |
| POSITION_STREAM | 0x15 | `MPOS` | Motor position data |

### New Tags (not in old protocol)

| Tag | Description |
|-----|-------------|
| `IMU0` | IMU data (accel x,y,z) |
| `RDAR` | Radar target data |
| `BHVR` | Behavior control (enable/disable) |
| `BLST` | Behavior list (list all behaviors and states) |
| `VLST` | List all visemes with names and motor positions |
| `VADD` | Add a new viseme with name |
| `VDEL` | Delete a viseme by ID |
| `VSET` | Set motor positions for a viseme |
| `VSME` | Trigger viseme (fire-and-forget) |
| `STAT` | System state/heartbeat |
| `FACE` | Face detection data (Radxa → host/robot, via WiFi) |
| `ALIV` | Remote component alive/heartbeat (component ID + status) |
| `SSET` | Settings set/dump (read/write individual settings by ID) |
| `ACK!` | Acknowledge (success) |
| `NACK` | Negative acknowledge (failure) |
| `BOOT` | Enter bootloader |

---

## Detailed Command Reference

### Identity & Configuration

#### `IDNT` - Get Robot Identity
**Request:** Empty payload  
**Response:** Robot config serialized bytes (same format as before)

#### `CONF` - Update Configuration
**Request:** Same payload format as old CMD_WRITE_CONFIG_UPDATE  
**Response:** `ACK!` on success, `NACK` with reason on failure

---

### File Operations

#### `FLST` - List Files
**Request:** Empty payload  
**Response:** Newline-separated filename list (UTF-8 string)

#### `FLOD` - Load File
**Request:** Filename as raw bytes (no length prefix)  
**Response:** File contents as raw bytes, or `NACK` if not found

#### `FSAV` - Save Animation
**Request:** Same format as old CMD_SAVE_FILE:
```
[filename_len: 2 bytes LE]
[filename: N bytes]
[animation_header: 18 bytes]
[curve_segments: variable]
[node_graph: variable]
```
**Response:** `ACK!` on success, `NACK` on failure

#### `FDEL` - Delete File
**Request:**
```
[filename_len: 2 bytes LE]
[filename: N bytes]
```
**Response:** `ACK!` on success

#### `FPLY` - Play Animation
**Request:**
```
[filename_len: 2 bytes LE]
[filename: N bytes]
[play_mode: 1 byte]   // 0=idle, 1=once, 2=loop, 3=repeat
[repeat_count: 1 byte]
[start_frame: 2 bytes LE]  // Frame number to start playback from (0-based)
```
**Response:** `ACK!` on success, `NACK` if file not found

**Notes:**
- `start_frame` allows resuming playback from a specific frame
- FRAME packets report actual frame numbers (i.e., if start_frame=163, FRAME packets will show 163, 164, 165...)

#### `FSTP` - Stop Animation
**Request:** Empty payload (0 bytes)
**Response:** `ACK!` on success

**Notes:**
- Immediately stops the currently playing animation regardless of play mode
- Motors remain in their current positions (torque not disabled)
- No FRAME completion packet is sent

---

### Motor Control

#### `MSET` - Set Motor Positions
**Request:** Array of motor commands:
```
[motor_id: 1 byte][position: 2 bytes LE] × N motors
```
**Response:** `ACK!`

#### `MPOS` - Motor Position Stream (device → host)
**Payload:** Same format as MSET request
```
[motor_id: 1 byte][position: 2 bytes LE] × N motors
```
*Sent automatically when streaming is enabled*

#### `MSCN` - Scan for Motors
**Request:**
```
[channel: 1 byte]  // 0 or 1
```
**Response:** Multiple packets, one per found motor:
```
[channel: 1][motor_id: 1][model: 2][min_angle: 2][max_angle: 2]
[position: 2][cw_dead: 1][ccw_dead: 1][offset: 2][mode: 1]
[torque_enable: 1][acceleration: 1][goal_pos: 2][goal_time: 2]
[goal_speed: 2][lock: 1][speed: 2][load: 2][temp: 1][moving: 1]
[current: 2][voltage: 1]
```
Final packet has `motor_id = 255` to signal scan complete.

#### `MWRT` - Write Motor Register
**Request:**
```
[channel: 1 byte]
[motor_id: 1 byte]
[register: 1 byte]
[data_len: 1 byte]   // 1 or 2
[data: 1-2 bytes]
```
**Response:** Register read-back value (1 or 2 bytes)

*Special case:* Register 5 with 1 byte changes the motor ID.

#### `MSTM` - Motor Stream Control
**Request:**
```
[enable: 1 byte]  // 0=disable, 1=enable
```
**Response:** `ACK!`

When enabled, device streams `MPOS` packets every 50ms.

---

### Sensors

#### `IMU0` - IMU Data (device → host)
**Payload:**
```
[accelX: 2 bytes LE, signed]   // g-forces × 100
[accelY: 2 bytes LE, signed]   // g-forces × 100
[accelZ: 2 bytes LE, signed]   // g-forces × 100
[pitch: 2 bytes LE, signed]    // degrees × 100
[roll: 2 bytes LE, signed]     // degrees × 100
```
*Sent automatically when IMU streaming is enabled*

**Coordinate System:**
- X: left/right axis (affects roll)
- Y: front/back axis (affects pitch)
- Z: up/down axis

**Notes:**
- Acceleration values scaled by 100 (not 1000 as before)
- Euler angles calculated from accelerometer data
- Heading/yaw not available (accelerometer only)

#### `RDAR` - Radar Data (device → host)
**Payload:**
```
[target_count: 1 byte]
For each of 3 targets:
  [valid: 1 byte]      // 0 or 1
  [x: 2 bytes LE]      // cm × 10, signed
  [y: 2 bytes LE]      // cm × 10, signed
  [speed: 2 bytes LE]  // cm/s × 10, signed
```
*Sent automatically when radar streaming is enabled*

#### `FACE` - Face Detection Data (Radxa → host/robot via WiFi)
**Payload:**
```
[face_count: 1 byte]   // 0–N faces
For each face:
  [x: 2 bytes LE, signed]   // center-relative pixels (0,0 = image center)
  [y: 2 bytes LE, signed]
  [w: 2 bytes LE]           // bounding box width
  [h: 2 bytes LE]           // bounding box height
  [conf: 1 byte]            // confidence × 255 (0–255)
```

**Notes:**
- Sent from Radxa over WebSocket when faces are detected
- Radxa runs `face_server.py` (WebSocket server); robot/host connects to receive packets
- x, y are offset from image center: positive = right/down, negative = left/up
- Same packet format as serial (0xA5 0x5A + TAG + LENGTH + SEQ + PAYLOAD + CRC16)

#### `ALIV` - Remote Component Alive (Radxa/other → host/robot via WiFi)
**Payload:**
```
[component_id: 1 byte]   // Assigned ID for each remote component
[alive: 1 byte]         // 0 = not alive / stream disconnected, 1 = alive
// Future: extended payload for other devices
```

**Component IDs:**
- `3` = Face detection (Radxa `face_server.py` – alive = MJPEG stream connected)

**Notes:**
- Sent every 2 seconds by each remote component
- Use `alive` to detect when a component has lost its connection and is retrying

---

### Behaviors

#### `BHVR` - Behavior Control (host → device)
**Request:**
```
[behaviorID: 1 byte]   // Behavior ID (1 = Focus)
[enable: 1 byte]       // 0 = disable, non-zero = enable
```
**Response:** `ACK!` on success, `NACK` on failure

**Behavior IDs:**
- `1` = Focus (face tracking with eye motors 14 & 15)
- `2` = Idle (perlin noise motion for all motors, ±500 range from center)
- `3` = Viseme (mouth motor control for speech)

#### `BLST` - Behavior List (host → device)
**Request:** Empty payload

**Response:**
```
[count: 1 byte]                    // Number of registered behaviors
[behaviorID1: 1 byte][enabled1: 1 byte]  // First behavior
[behaviorID2: 1 byte][enabled2: 1 byte]  // Second behavior
...
```
- `enabled`: 1 = enabled, 0 = disabled

---

### Visemes

#### `VLST` - List Visemes (host → device)
**Request:** Empty payload

**Response:**
```
[count: 1 byte]                    // Number of visemes
For each viseme:
  [viseme_id: 1 byte]
  [label: 3 bytes]                  // 3-character label (e.g., "AA ", "SIL")
  [motor_count: 1 byte]
  For each motor:
    [motor_id: 1 byte]
    [position_low: 1 byte]
    [position_high: 1 byte]
```
Position = `position_low | (position_high << 8)`, range 0-4095

#### `VADD` - Add Viseme (host → device)
**Request:**
```
[label: 3 bytes]                    // 3-character label (e.g., "AA ", "SIL")
```

**Response:** `ACK!` with payload `[new_viseme_id: 1 byte]` on success, `NACK` on failure

#### `VDEL` - Delete Viseme (host → device)
**Request:**
```
[viseme_id: 1 byte]
```

**Response:** `ACK!` on success, `NACK` if viseme not found

#### `VSET` - Set Viseme Motor Positions (host → device)
**Request:**
```
[viseme_id: 1 byte]
[motor_count: 1 byte]
For each motor:
  [motor_id: 1 byte]
  [position_low: 1 byte]
  [position_high: 1 byte]
```

**Response:** `ACK!` on success, `NACK` if viseme not found

#### `VSME` - Trigger Viseme (host → device)
**Request:**
```
[viseme_id: 1 byte]
```

**Response:** None (fire-and-forget)

**Notes:**
- Triggers the viseme behavior which controls the motors defined for that viseme
- The behavior activates and holds the motor positions
- After 3 seconds without a new viseme trigger, the behavior deactivates and releases motor control
- Continuously sending viseme IDs will keep the mouth animated
- Viseme behavior has higher priority than Idle behavior, lower than Focus

**Default Viseme IDs (loaded at startup):**
- `0` = Neutral/rest (sil) - motors 40, 43, 44
- `1` = AA (as in "father")
- `2` = AE (as in "cat")
- `3` = AH (as in "but")
- `4` = AO (as in "bought")
- `5` = EH (as in "bed")
- `6` = IH (as in "bit")
- `7` = IY (as in "beat")
- `8` = OW (as in "boat")
- `9` = UH (as in "book")
- `10` = UW (as in "boot")

---

### Settings

#### `SSET` - Settings Set/Dump (host ↔ device)

**Write a single setting:**
**Request:**
```
[setting_id: 2 bytes LE]
[data: N bytes]          // 2 bytes for numeric, variable for strings
```
**Response:** `ACK!` on success, `NACK` if unknown setting ID

**Notes:** Writing a WiFi/WebSocket setting triggers an automatic reconnect.

**Dump all settings:**
**Request:** Empty payload (0 bytes)
**Response:** `SSET` packet:
```
[count: 2 bytes LE]
For each setting:
  [setting_id: 2 bytes LE]
  [data_len: 2 bytes LE]
  [data: data_len bytes]
```

**Value encoding:**
- `uint8`/`uint16`/`bool`: `data_len=2`, stored as uint16 LE
- `float` (0.0–65.535): `data_len=2`, stored as `value × 1000` (e.g., 0.15 → 150)
- `int16` (signed): `data_len=2`, stored as uint16 reinterpret (e.g., -140 → 0xFF74)
- `string`: `data_len=N`, raw UTF-8 bytes (no null terminator)

**Focus Behavior Setting IDs (0x0500–0x0512):**

| ID | Name | Type | Default | Description |
|----|------|------|---------|-------------|
| `0x0500` | FOCUS_EYE_MOTOR_1 | uint8 | 14 | Eye motor 1 ID |
| `0x0501` | FOCUS_EYE_MOTOR_2 | uint8 | 15 | Eye motor 2 ID |
| `0x0502` | FOCUS_NECK_MOTOR | uint8 | 27 | Neck yaw motor ID |
| `0x0503` | FOCUS_EYE_CENTER | uint16 | 2200 | Eye servo center position |
| `0x0504` | FOCUS_EYE_MIN | uint16 | 1700 | Eye servo min position |
| `0x0505` | FOCUS_EYE_MAX | uint16 | 2500 | Eye servo max position |
| `0x0506` | FOCUS_NECK_CENTER | uint16 | 2000 | Neck servo center position |
| `0x0507` | FOCUS_NECK_MIN | uint16 | 1000 | Neck servo min position |
| `0x0508` | FOCUS_NECK_MAX | uint16 | 3000 | Neck servo max position |
| `0x0509` | FOCUS_FACE_X_MIN | int16 | -140 | Face detection X min (pixels) |
| `0x050A` | FOCUS_FACE_X_MAX | int16 | 140 | Face detection X max (pixels) |
| `0x050B` | FOCUS_EYE_SPEED | float×1000 | 150 | Eye dart speed (0–1) |
| `0x050C` | FOCUS_NECK_SPEED | float×1000 | 20 | Neck follow speed (0–1) |
| `0x050D` | FOCUS_EYE_RETURN_SPEED | float×1000 | 50 | Eye return-to-center speed |
| `0x050E` | FOCUS_NECK_DELAY_MS | uint16 | 500 | Neck start delay (ms) |
| `0x050F` | FOCUS_NECK_CONTRIBUTION | float×1000 | 700 | Neck offset coverage (0–1) |
| `0x0510` | FOCUS_NECK_INVERT | bool | 1 | Invert neck motor direction |
| `0x0511` | FOCUS_EYE_CENTERING | float×1000 | 30 | Eye centering speed (no face) |
| `0x0512` | FOCUS_NECK_CENTERING | float×1000 | 20 | Neck centering speed (no face) |

**WiFi / WebSocket Setting IDs (0x0600–0x0604):**

| ID | Name | Type | Default | Description |
|----|------|------|---------|-------------|
| `0x0600` | WIFI_SSID | string | "Police Surveillance Van" | WiFi network name (max 32 chars) |
| `0x0601` | WIFI_PASSWORD | string | "ourpassword" | WiFi password (max 64 chars) |
| `0x0602` | WIFI_HOST | string | "192.168.1.206" | WebSocket server IP/hostname (max 63 chars) |
| `0x0603` | WIFI_PORT | uint16 | 5001 | WebSocket server port |
| `0x0604` | WIFI_PATH | string | "/" | WebSocket URL path (max 31 chars) |

**Notes:**
- All settings are persisted to flash on write
- The dump includes all registered settings with a length prefix per entry
- Setting ranges are extensible: `0x0500` = Focus, `0x0600` = WiFi, future = `0x0700+`

---

### System

#### `STAT` - System State/Heartbeat (device → host)
**Payload:**
```
[uptime: 4 bytes LE]  // seconds since boot
[flags: 2 bytes LE]   // bit flags
```
**Flags:**
- Bit 0: IMU ready
- Bit 1: Animation playing
- Bit 2: Motor streaming active
- Bit 3: IMU streaming active
- Bit 4: Radar streaming active
- Bit 5: Face streaming active
- Bit 6: Face detection remote alive

*Sent automatically every 1 second*

#### `MSGE` - Log Message (device → host)
**Payload:** UTF-8 string (no null terminator)

#### `ACK!` - Acknowledge
**Payload:**
```
[original_tag: 4 bytes]  // The tag being acknowledged
```

#### `NACK` - Negative Acknowledge
**Payload:**
```
[original_tag: 4 bytes]
[reason: N bytes, optional UTF-8 string]
```

#### `BOOT` - Enter Bootloader
**Request:** Empty payload  
**Response:** `MSGE` "Entering bootloader...", then device resets

---

## Implementation Checklist

### 1. Protocol Layer Changes

- [ ] Update sync byte detection from `0xAA 0x55` to `0xA5 0x5A`
- [ ] Implement CRC16-CCITT calculation
- [ ] Update packet parsing to handle new format:
  - [ ] Read 4-byte tag instead of 1-byte command
  - [ ] Read 2-byte sequence number (can ignore for now, or use for debugging)
  - [ ] Verify CRC16 instead of XOR checksum
- [ ] Update packet building:
  - [ ] Use 4-byte tags
  - [ ] Add sequence counter (increment per packet)
  - [ ] Calculate and append CRC16

### 2. Command Handler Updates

- [ ] Replace command ID constants with tag strings
- [ ] Update request builders for each command
- [ ] Update response parsers for each command
- [ ] Add handlers for new response types:
  - [ ] `ACK!` - generic success
  - [ ] `NACK` - generic failure with reason
  - [ ] `STAT` - heartbeat (can use to detect connection)
  - [ ] `IMU0` - IMU data (if needed)
  - [ ] `RDAR` - radar data (if needed)

### 3. UI/UX Improvements (Optional)

- [ ] Show connection status based on `STAT` heartbeat
- [ ] Display IMU orientation if sensor is available
- [ ] Show radar targets if sensor is available

---

## Example: Sending a Motor Position Command

### Old Code (pseudocode)
```python
def send_motor_positions(motors):
    payload = b''
    for motor_id, position in motors:
        payload += bytes([motor_id])
        payload += struct.pack('<H', position)
    
    length = len(payload)
    checksum = CMD_SET_POSITION ^ (length >> 8) ^ (length & 0xFF)
    for b in payload:
        checksum ^= b
    
    packet = bytes([0xAA, 0x55, CMD_SET_POSITION])
    packet += struct.pack('>H', length)  # big-endian length
    packet += payload
    packet += bytes([checksum])
    
    serial.write(packet)
```

### New Code (pseudocode)
```python
def send_motor_positions(motors):
    tag = b'MSET'
    payload = b''
    for motor_id, position in motors:
        payload += bytes([motor_id])
        payload += struct.pack('<H', position)
    
    length = len(payload)
    seq = get_next_sequence()
    
    # Build data for CRC: tag + length + seq + payload
    crc_data = tag
    crc_data += struct.pack('<H', length)
    crc_data += struct.pack('<H', seq)
    crc_data += payload
    crc = crc16_ccitt(crc_data)
    
    packet = bytes([0xA5, 0x5A])
    packet += tag
    packet += struct.pack('<H', length)
    packet += struct.pack('<H', seq)
    packet += payload
    packet += struct.pack('<H', crc)
    
    serial.write(packet)
```

---

## Testing Strategy

1. **Connection Test:** Send empty `IDNT` request, expect `IDNT` response with config
2. **File List Test:** Send `FLST`, expect filename list
3. **Motor Test:** Send `MSET` with known positions, expect `ACK!`
4. **Heartbeat:** After connecting, should receive `STAT` packets every second

---

## Backwards Compatibility

The new protocol uses different sync bytes (`0xA5 0x5A` vs `0xAA 0x55`), so there's no ambiguity. If you need to support both old and new firmware:

1. Detect firmware version by sync bytes in received packets
2. Switch protocol handler based on detected version
3. Or: just update all firmware to new version

---

## Questions?

The firmware source is at:
`C:\Users\jake\Documents\hansonProjects\HansonServo\`

Key files:
- `protocol.h/cpp` - Packet format and CRC implementation
- `commands.h/cpp` - Command handlers
- `sensors.h/cpp` - IMU and radar drivers
- `websocket_client.h/cpp` - WiFi, WebSocket client + server

---

## WebSocket Server

The firmware runs a WebSocket server on port **81** alongside the outbound client.
Any device on the same WiFi network can connect and send protocol-framed packets.

**URL:** `ws://<robot-ip>:81/`

All commands from the serial protocol are supported over WebSocket:
`VSME`, `MSET`, `BHVR`, `SSET`, `FPLY`, `FSTP`, etc.

Packets must use the same binary framing as serial:
`0xA5 0x5A + TAG(4) + LEN(2) + SEQ(2) + PAYLOAD(N) + CRC16(2)`

Up to 2 simultaneous WebSocket client connections are supported.