674 lines
20 KiB
Markdown
674 lines
20 KiB
Markdown
# 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.
|
||
|