Major changes to bring in line with protocol v2

PROTOCOL_MIGRATION.md

@@ -0,0 +1,404 @@
+# 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_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 (heading, roll, pitch) |
+| `RDAR` | Radar target data |
+| `STAT` | System state/heartbeat |
+| `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]
+```
+**Response:** `ACK!` on success, `NACK` if file not found
+
+---
+
+### 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:**
+```
+[heading: 2 bytes LE, signed]  // degrees × 100
+[roll: 2 bytes LE, signed]     // degrees × 100
+[pitch: 2 bytes LE, signed]    // degrees × 100
+```
+*Sent automatically when IMU streaming is enabled*
+
+#### `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*
+
+---
+
+### 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
+
+*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
+

commands.cpp

@@ -0,0 +1,555 @@
+#include "commands.h"
+#include "nodegraph.h"
+#include "sensors.h"
+#include "esp_system.h"
+#include "soc/rtc_cntl_reg.h"
+#include <vector>
+
+// External references
+extern RobotConfig config;
+extern ServoManager servoManager;
+
+// Global state instances
+AnimationState animState;
+MotorStreamState motorStream;
+
+// ============================================================================
+// AnimationState
+// ============================================================================
+
+void AnimationState::play(PlayMode mode, uint8_t repeats) {
+  current = &animation;
+  current->setActive(true);
+  playMode = mode;
+  repeatsRemaining = repeats;
+}
+
+void AnimationState::stop() {
+  if (current) {
+    current->setActive(false);
+  }
+  playMode = PLAY_IDLE;
+}
+
+// ============================================================================
+// MotorStreamState
+// ============================================================================
+
+void MotorStreamState::start() {
+  active = true;
+  lastStreamTime = millis();
+}
+
+void MotorStreamState::stop() {
+  active = false;
+}
+
+bool MotorStreamState::shouldStream() {
+  if (!active) return false;
+  if (millis() - lastStreamTime >= STREAM_INTERVAL_MS) {
+    lastStreamTime = millis();
+    return true;
+  }
+  return false;
+}
+
+// ============================================================================
+// Command Dispatcher
+// ============================================================================
+
+void dispatchCommand() {
+  const char* tag = getReceivedTag();
+  const uint8_t* payload = getReceivedPayload();
+  uint16_t len = getReceivedPayloadLen();
+  
+  // Identity & Config
+  if (tagMatches(tag, Tag::IDENT)) {
+    handleIdent(payload, len);
+  }
+  else if (tagMatches(tag, Tag::CONFG)) {
+    handleConfig(payload, len);
+  }
+  // File Operations
+  else if (tagMatches(tag, Tag::FLIST)) {
+    handleFileList(payload, len);
+  }
+  else if (tagMatches(tag, Tag::FLOAD)) {
+    handleFileLoad(payload, len);
+  }
+  else if (tagMatches(tag, Tag::FSAVE)) {
+    handleFileSave(payload, len);
+  }
+  else if (tagMatches(tag, Tag::FDELE)) {
+    handleFileDelete(payload, len);
+  }
+  else if (tagMatches(tag, Tag::FPLAY)) {
+    handleFilePlay(payload, len);
+  }
+  // Motor Control
+  else if (tagMatches(tag, Tag::MSET)) {
+    handleMotorSet(payload, len);
+  }
+  else if (tagMatches(tag, Tag::MSCAN)) {
+    handleMotorScan(payload, len);
+  }
+  else if (tagMatches(tag, Tag::MWRIT)) {
+    handleMotorWrite(payload, len);
+  }
+  else if (tagMatches(tag, Tag::MSTRM)) {
+    handleMotorStream(payload, len);
+  }
+  // System
+  else if (tagMatches(tag, Tag::BOOT)) {
+    handleBootloader(payload, len);
+  }
+  else {
+    char tagStr[5] = {tag[0], tag[1], tag[2], tag[3], 0};
+    sendMessage("Unknown tag: " + String(tagStr));
+  }
+}
+
+// ============================================================================
+// Identity & Config Handlers
+// ============================================================================
+
+void handleIdent(const uint8_t* payload, uint16_t len) {
+  std::vector<uint8_t> response = config.serializeToBytes();
+  sendPacket(Tag::IDENT, response.data(), response.size());
+}
+
+void handleConfig(const uint8_t* payload, uint16_t len) {
+  RobotConfig newConfig;
+  uint16_t offset = 0;
+
+  // Decode robot name
+  if (offset >= len) { sendNack(Tag::CONFG, "Missing name"); return; }
+  uint8_t nameLength = payload[offset++];
+  for (uint8_t i = 0; i < nameLength && offset < len; ++i) {
+    newConfig.deviceName += (char)payload[offset++];
+  }
+
+  // Decode firmware version
+  if (offset + 2 > len) { sendNack(Tag::CONFG, "Missing version"); return; }
+  newConfig.firmwareVersion.major = payload[offset++];
+  newConfig.firmwareVersion.minor = payload[offset++];
+
+  // Decode motor count
+  if (offset >= len) { sendNack(Tag::CONFG, "Missing motors"); return; }
+  uint8_t motorCount = payload[offset++];
+
+  // Decode motors
+  for (uint8_t i = 0; i < motorCount && offset < len; ++i) {
+    if (offset + 5 > len) break;
+
+    ServoModel model;
+    model.major = payload[offset++];
+    model.minor = payload[offset++];
+
+    uint16_t motorID = payload[offset++] | (payload[offset++] << 8);
+
+    uint8_t motorNameLength = payload[offset++];
+    String motorName = "";
+    for (uint8_t j = 0; j < motorNameLength && offset < len; ++j) {
+      motorName += (char)payload[offset++];
+    }
+
+    Motor motor;
+    motor.motorID = motorID;
+    motor.servoModel = model;
+    motor.name = motorName;
+    motor.position = 0;
+    motor.isEnabled = true;
+
+    newConfig.motors.push_back(motor);
+  }
+
+  config = newConfig;
+  config.saveToFFat();
+  
+  sendAck(Tag::CONFG);
+  sendMessage("Config updated: " + config.deviceName);
+}
+
+// ============================================================================
+// File Operation Handlers
+// ============================================================================
+
+void handleFileList(const uint8_t* payload, uint16_t len) {
+  File root = FFat.open("/");
+  if (!root || !root.isDirectory()) {
+    sendPacket(Tag::FLIST, nullptr, 0);
+    return;
+  }
+
+  String fileList = "";
+  File file = root.openNextFile();
+  while (file) {
+    if (!file.isDirectory()) {
+      fileList += String(file.name()) + "\n";
+    }
+    file = root.openNextFile();
+  }
+
+  sendPacket(Tag::FLIST, (const uint8_t*)fileList.c_str(), fileList.length());
+}
+
+void handleFileLoad(const uint8_t* payload, uint16_t len) {
+  if (len == 0 || len >= 128) {
+    sendNack(Tag::FLOAD, "Invalid filename");
+    return;
+  }
+
+  char filename[128];
+  memcpy(filename, payload, len);
+  filename[len] = '\0';
+
+  File file = FFat.open(filename, "r");
+  if (!file || !file.available()) {
+    sendNack(Tag::FLOAD, "File not found");
+    return;
+  }
+
+  size_t fileSize = file.size();
+  if (fileSize > MAX_PAYLOAD_SIZE) {
+    sendNack(Tag::FLOAD, "File too large");
+    file.close();
+    return;
+  }
+
+  uint8_t* buffer = (uint8_t*)malloc(fileSize);
+  if (!buffer) {
+    sendNack(Tag::FLOAD, "Out of memory");
+    file.close();
+    return;
+  }
+
+  file.read(buffer, fileSize);
+  file.close();
+
+  sendPacket(Tag::FLOAD, buffer, fileSize);
+  free(buffer);
+}
+
+void handleFileSave(const uint8_t* payload, uint16_t len) {
+  if (parseAndSaveAnimation(payload, len, animState.animation)) {
+    sendAck(Tag::FSAVE);
+  } else {
+    sendNack(Tag::FSAVE, "Parse failed");
+  }
+}
+
+void handleFileDelete(const uint8_t* payload, uint16_t len) {
+  if (len < 2) {
+    sendNack(Tag::FDELE, "Invalid request");
+    return;
+  }
+  
+  uint16_t filenameLen = payload[0] | (payload[1] << 8);
+  if (len < 2 + filenameLen) {
+    sendNack(Tag::FDELE, "Invalid filename");
+    return;
+  }
+
+  char filename[128];
+  memcpy(filename, payload + 2, min((uint16_t)127, filenameLen));
+  filename[min((uint16_t)127, filenameLen)] = '\0';
+
+  String fullPath = "/" + String(filename);
+  deleteFile(FFat, fullPath.c_str());
+  
+  sendAck(Tag::FDELE);
+  sendMessage("Deleted: " + String(filename));
+}
+
+void handleFilePlay(const uint8_t* payload, uint16_t len) {
+  if (len < 4) {
+    sendNack(Tag::FPLAY, "Invalid request");
+    return;
+  }
+  
+  uint16_t filenameLen = payload[0] | (payload[1] << 8);
+  if (len < 2 + filenameLen + 2) {
+    sendNack(Tag::FPLAY, "Invalid filename");
+    return;
+  }
+
+  char filename[128];
+  memcpy(filename, payload + 2, min((uint16_t)127, filenameLen));
+  filename[min((uint16_t)127, filenameLen)] = '\0';
+
+  uint16_t offset = 2 + filenameLen;
+  PlayMode mode = static_cast<PlayMode>(payload[offset]);
+  uint8_t repeats = payload[offset + 1];
+
+  animState.animation.clear();
+  String fullPath = "/" + String(filename);
+  
+  if (animState.animation.loadFromFile(fullPath.c_str())) {
+    animState.play(mode, repeats);
+    sendAck(Tag::FPLAY);
+    sendMessage("Playing: " + String(filename));
+  } else {
+    sendNack(Tag::FPLAY, "Load failed");
+  }
+}
+
+// ============================================================================
+// Motor Control Handlers
+// ============================================================================
+
+void handleMotorSet(const uint8_t* payload, uint16_t len) {
+  if (len % 3 != 0) {
+    sendNack(Tag::MSET, "Invalid length");
+    return;
+  }
+
+  uint8_t motorCount = len / 3;
+  uint8_t ids[motorCount];
+  uint16_t positions[motorCount];
+  uint16_t speeds[motorCount];
+
+  for (uint8_t i = 0; i < motorCount; ++i) {
+    ids[i] = payload[i * 3];
+    positions[i] = (payload[i * 3 + 2] << 8) | payload[i * 3 + 1];
+    speeds[i] = 0;
+  }
+
+  servoManager.syncWritePositions(ids, positions, speeds, motorCount, config, 0);
+  sendAck(Tag::MSET);
+}
+
+void handleMotorScan(const uint8_t* payload, uint16_t len) {
+  if (len < 1) {
+    sendNack(Tag::MSCAN, "Invalid request");
+    return;
+  }
+
+  uint8_t channel = payload[0];
+  Feetech* servo = servoManager[channel];
+
+  for (int i = 0; i < 254; i++) {
+    servo->sendPing(i);
+    uint8_t val = servo->waitOnData1Byte(10);
+
+    if (val != 0) {
+      uint8_t response[33];
+      response[0] = channel;
+      response[1] = i;
+
+      uint16_t model = servo->getModel(i);
+      servo->setFeetechMode(model);
+      
+      uint16_t minAngle = servo->getMinAngleLimit(i);
+      uint16_t maxAngle = servo->getMaxAngleLimit(i);
+      uint16_t position = servo->getPosition(i);
+      uint8_t cwDead = servo->getCWDeadZone(i);
+      uint8_t ccwDead = servo->getCCWDeadZone(i);
+      uint16_t offset = servo->getOffset(i);
+      uint8_t mode = servo->getMode(i);
+      uint8_t torque = servo->getTorqueEnable(i);
+      uint8_t accel = servo->getAcceleration(i);
+      uint16_t goalPos = servo->getGoalPosition(i);
+      uint16_t goalTime = servo->getGoalTime(i);
+      uint16_t goalSpeed = servo->getGoalSpeed(i);
+      uint8_t lock = servo->getLock(i);
+      int16_t speed = servo->getSpeed(i);
+      uint16_t load = servo->getLoad(i);
+      uint8_t temp = servo->getTemperature(i);
+      uint8_t moving = servo->getMoving(i);
+      uint8_t current = servo->getCurrent(i);
+      uint8_t voltage = servo->getVoltage(i);
+
+      // Pack response
+      response[2] = (model >> 8) & 0xFF;
+      response[3] = model & 0xFF;
+      response[4] = (minAngle >> 8) & 0xFF;
+      response[5] = minAngle & 0xFF;
+      response[6] = (maxAngle >> 8) & 0xFF;
+      response[7] = maxAngle & 0xFF;
+      response[8] = (position >> 8) & 0xFF;
+      response[9] = position & 0xFF;
+      response[10] = cwDead;
+      response[11] = ccwDead;
+      response[12] = (offset >> 8) & 0xFF;
+      response[13] = offset & 0xFF;
+      response[14] = mode;
+      response[15] = torque;
+      response[16] = accel;
+      response[17] = (goalPos >> 8) & 0xFF;
+      response[18] = goalPos & 0xFF;
+      response[19] = (goalTime >> 8) & 0xFF;
+      response[20] = goalTime & 0xFF;
+      response[21] = (goalSpeed >> 8) & 0xFF;
+      response[22] = goalSpeed & 0xFF;
+      response[23] = lock;
+      response[24] = (speed >> 8) & 0xFF;
+      response[25] = speed & 0xFF;
+      response[26] = (load >> 8) & 0xFF;
+      response[27] = load & 0xFF;
+      response[28] = temp;
+      response[29] = moving;
+      response[30] = (current >> 8) & 0xFF;
+      response[31] = current & 0xFF;
+      response[32] = voltage;
+
+      sendPacket(Tag::MSCAN, response, 33);
+    }
+  }
+
+  // Signal end of scan
+  uint8_t endResponse[2] = { channel, 255 };
+  sendPacket(Tag::MSCAN, endResponse, 2);
+}
+
+void handleMotorWrite(const uint8_t* payload, uint16_t len) {
+  if (len < 5) {
+    sendNack(Tag::MWRIT, "Invalid request");
+    return;
+  }
+
+  uint8_t channel = payload[0];
+  uint8_t id = payload[1];
+  uint8_t reg = payload[2];
+  uint8_t dataLen = payload[3];
+
+  Feetech* servo = servoManager[channel];
+  uint8_t model = servo->getMajorModel(id);
+  servo->setFeetechMode(model);
+
+  // Special case for ID changes (reg 5)
+  if (reg == 5 && dataLen == 1) {
+    servo->changeID(id, payload[4]);
+    sendAck(Tag::MWRIT);
+    sendMessage("ID changed");
+    return;
+  }
+
+  if (dataLen == 1) {
+    servo->write1Byte(id, reg, payload[4]);
+    uint8_t response = servo->waitOnData1Byte(10);
+    sendPacket(Tag::MWRIT, &response, 1);
+  } else {
+    servo->write2Bytes(id, reg, payload[4] | (payload[5] << 8));
+    uint16_t response = servo->waitOnData2Bytes(10);
+    uint8_t respBuf[2] = { (uint8_t)(response & 0xFF), (uint8_t)(response >> 8) };
+    sendPacket(Tag::MWRIT, respBuf, 2);
+  }
+}
+
+void handleMotorStream(const uint8_t* payload, uint16_t len) {
+  if (len < 1) {
+    sendNack(Tag::MSTRM, "Invalid request");
+    return;
+  }
+
+  bool enable = payload[0] != 0;
+
+  if (enable) {
+    // Disable torque for position reading
+    for (const Motor& motor : config.motors) {
+      servoManager[0]->setFeetechMode(motor.servoModel.major);
+      servoManager[0]->disableTorque(motor.motorID);
+    }
+    motorStream.start();
+  } else {
+    motorStream.stop();
+  }
+
+  sendAck(Tag::MSTRM);
+}
+
+// ============================================================================
+// System Handlers
+// ============================================================================
+
+void handleBootloader(const uint8_t* payload, uint16_t len) {
+  sendMessage("Entering bootloader...");
+  Serial.flush();
+  delay(100);
+  
+  REG_WRITE(RTC_CNTL_OPTION1_REG, RTC_CNTL_FORCE_DOWNLOAD_BOOT);
+  esp_restart();
+}
+
+// ============================================================================
+// Helper Functions
+// ============================================================================
+
+void sendMotorPositions() {
+  std::vector<uint8_t> payload;
+
+  for (const Motor& motor : config.motors) {
+    payload.push_back(motor.motorID);
+    payload.push_back(motor.position & 0xFF);
+    payload.push_back((motor.position >> 8) & 0xFF);
+  }
+
+  sendPacket(Tag::MPOS, payload.data(), payload.size());
+}
+
+bool parseAndSaveAnimation(const uint8_t* payload, uint16_t len, Animation& animation) {
+  if (len < 2) return false;
+  
+  uint16_t filenameLen = payload[0] | (payload[1] << 8);
+  if (len < 2 + filenameLen + 18) return false;
+
+  char filename[128];
+  memcpy(filename, payload + 2, min((uint16_t)127, filenameLen));
+  filename[min((uint16_t)127, filenameLen)] = '\0';
+
+  const uint8_t* ptr = payload + 2 + filenameLen;
+  uint16_t remaining = len - 2 - filenameLen;
+
+  sendMessage("Saving: " + String(filename));
+
+  // Parse header
+  if (remaining < 18) return false;
+  memcpy(animation.header.magic, ptr, 4);
+  if (strncmp(animation.header.magic, "ANIM", 4) != 0) {
+    sendMessage("Invalid magic header");
+    return false;
+  }
+
+  animation.header.frameCount = ptr[4] | (ptr[5] << 8);
+  animation.header.version = ptr[6];
+  animation.header.frameRate = ptr[7];
+  memcpy(animation.header.reserved, ptr + 8, 8);
+
+  uint16_t curveCount = ptr[16] | (ptr[17] << 8);
+  ptr += 18;
+  remaining -= 18;
+
+  // Parse curves
+  uint16_t curveDataSize = curveCount * sizeof(CurveSegment);
+  if (remaining < curveDataSize) return false;
+
+  animation.clearAllCurves();
+  for (uint16_t i = 0; i < curveCount; i++) {
+    CurveSegment seg;
+    memcpy(&seg, ptr, sizeof(CurveSegment));
+    animation.addCurveSegment(seg);
+    ptr += sizeof(CurveSegment);
+  }
+  remaining -= curveDataSize;
+
+  // Parse node graph
+  if (remaining > 0) {
+    loadNodeGraph(ptr, remaining, animation.nodeGraph);
+    animation.nodeGraph.bindAnimationContext(&animation);
+  }
+
+  // Save to file
+  String fullPath = "/" + String(filename);
+  animation.saveToFile(fullPath.c_str());
+  
+  sendMessage("Saved: " + String(filename));
+  return true;
+}
+
+void deleteFile(fs::FS& fs, const char* path) {
+  if (fs.remove(path)) {
+    Serial.printf("Deleted: %s\n", path);
+  } else {
+    Serial.printf("Delete failed: %s\n", path);
+  }
+}

commands.h

@@ -0,0 +1,83 @@
+#pragma once
+#include <Arduino.h>
+#include <FFat.h>
+#include "protocol.h"
+#include "animation.h"
+#include "RobotConfig.h"
+#include "motorcontrol.h"
+
+// ============================================================================
+// Animation State
+// ============================================================================
+
+struct AnimationState {
+  Animation animation;
+  Animation* current = nullptr;
+  PlayMode playMode = PLAY_IDLE;
+  uint8_t repeatsRemaining = 0;
+  
+  void play(PlayMode mode, uint8_t repeats = 0);
+  void stop();
+};
+
+extern AnimationState animState;
+
+// ============================================================================
+// Motor Position Streaming State
+// ============================================================================
+
+struct MotorStreamState {
+  bool active = false;
+  unsigned long lastStreamTime = 0;
+  static constexpr unsigned long STREAM_INTERVAL_MS = 50;
+  
+  void start();
+  void stop();
+  bool shouldStream();
+};
+
+extern MotorStreamState motorStream;
+
+// ============================================================================
+// Command Dispatcher
+// ============================================================================
+
+// Process a received packet - call after receivePacket() returns true
+void dispatchCommand();
+
+// ============================================================================
+// Individual Command Handlers
+// ============================================================================
+
+// Identity & Config
+void handleIdent(const uint8_t* payload, uint16_t len);
+void handleConfig(const uint8_t* payload, uint16_t len);
+
+// File Operations
+void handleFileList(const uint8_t* payload, uint16_t len);
+void handleFileLoad(const uint8_t* payload, uint16_t len);
+void handleFileSave(const uint8_t* payload, uint16_t len);
+void handleFileDelete(const uint8_t* payload, uint16_t len);
+void handleFilePlay(const uint8_t* payload, uint16_t len);
+
+// Motor Control
+void handleMotorSet(const uint8_t* payload, uint16_t len);
+void handleMotorScan(const uint8_t* payload, uint16_t len);
+void handleMotorWrite(const uint8_t* payload, uint16_t len);
+void handleMotorStream(const uint8_t* payload, uint16_t len);
+
+// System
+void handleBootloader(const uint8_t* payload, uint16_t len);
+
+// ============================================================================
+// Helper Functions
+// ============================================================================
+
+// Send current motor positions
+void sendMotorPositions();
+
+// Parse animation from payload and save to file
+bool parseAndSaveAnimation(const uint8_t* payload, uint16_t len, Animation& animation);
+
+// Delete a file
+void deleteFile(fs::FS& fs, const char* path);

motorcontrol.cpp

@@ -0,0 +1,60 @@
+#include "motorcontrol.h"
+
+// Global servo manager instance
+ServoManager servoManager;
+
+uint16_t flipBytes(uint16_t value) {
+  return (value >> 8) | (value << 8);
+}
+
+void prepareMotorPositions(
+  const uint8_t* ids,
+  uint16_t* positions,
+  uint16_t* speeds,
+  uint8_t count,
+  RobotConfig& config
+) {
+  for (uint8_t i = 0; i < count; i++) {
+    if (config.getMotorModel(ids[i]) == MODEL_STS) {
+      // STS servos: flip bytes, different speed range
+      positions[i] = flipBytes(positions[i]);
+      speeds[i] = map(speeds[i], 0, 4095, 0, 254);
+      speeds[i] = flipBytes(speeds[i]);
+    } else {
+      // SCS servos: map to 10-bit range
+      positions[i] = map(positions[i], 0, 4095, 0, 1023);
+      speeds[i] = map(speeds[i], 0, 4095, 0, 1000);
+    }
+  }
+}
+
+void ServoManager::init() {
+  servos[0] = new Feetech(Serial1, Pins::DE, Pins::RE, Pins::CH0_TX, Pins::CH0_RX);
+  servos[0]->setFeetechMode(Feetech::MODE_SCS);
+  servos[0]->begin();
+  
+  servos[1] = new Feetech(Serial2, Pins::DE, Pins::RE, Pins::CH1_TX, Pins::CH1_RX);
+  servos[1]->setFeetechMode(Feetech::MODE_SCS);
+  // servos[1]->begin();  // Uncomment when using channel 1
+}
+
+void ServoManager::syncWritePositions(
+  const uint8_t* ids,
+  uint16_t* positions,
+  uint16_t* speeds,
+  uint8_t count,
+  RobotConfig& config,
+  uint8_t channel
+) {
+  // Prepare positions (handles SCS vs STS conversion)
+  prepareMotorPositions(ids, positions, speeds, count, config);
+  
+  // Send to servos
+  servos[channel]->syncWritePos(
+    const_cast<uint8_t*>(ids),
+    positions,
+    speeds,
+    count
+  );
+}
+

motorcontrol.h

@@ -0,0 +1,60 @@
+#pragma once
+#include <Arduino.h>
+#include "feetech.h"
+#include "RobotConfig.h"
+
+// Pin definitions
+namespace Pins {
+  // Channel 0 (SCS servos)
+  constexpr int CH0_RX = 13;
+  constexpr int CH0_TX = 12;
+  
+  // Channel 1 (STS servos)  
+  constexpr int CH1_RX = 11;
+  constexpr int CH1_TX = 10;
+  
+  // RS485 control
+  constexpr int DE = 7;  // Driver Enable
+  constexpr int RE = 8;  // Receiver Enable
+}
+
+// Servo model constants
+constexpr uint8_t MODEL_STS = 9;
+
+// Utility functions
+uint16_t flipBytes(uint16_t value);
+
+// Prepare motor positions for transmission (handles SCS vs STS byte ordering)
+void prepareMotorPositions(
+  const uint8_t* ids,
+  uint16_t* positions,
+  uint16_t* speeds,
+  uint8_t count,
+  RobotConfig& config
+);
+
+// Servo manager class for cleaner access
+class ServoManager {
+public:
+  void init();
+  
+  Feetech* channel(uint8_t ch) { return servos[ch]; }
+  Feetech* operator[](uint8_t ch) { return servos[ch]; }
+  
+  // Convenience methods
+  void syncWritePositions(
+    const uint8_t* ids,
+    uint16_t* positions,
+    uint16_t* speeds,
+    uint8_t count,
+    RobotConfig& config,
+    uint8_t channel = 0
+  );
+
+private:
+  Feetech* servos[2] = {nullptr, nullptr};
+};
+
+// Global servo manager
+extern ServoManager servoManager;
+

nodegraph.cpp

@@ -2,9 +2,12 @@
 #include "animation.h"
 #include "noise.h"
 #include "RobotConfig.h"
+#include "sensors.h"
 #include <cstring>
 
 extern RobotConfig config;
+extern IMU imu;
+extern Radar radar;
 
 
 // CurveNode evaluation
@@ -61,6 +64,60 @@ void VariableNode::evaluate(uint32_t currentTick) {
     case VAR_SERVO_FEEDBACK:
       outputValue = config.getMotorPosition(arg0);
       break;
+      
+    // IMU sources - scale to 0-4095 range
+    case VAR_IMU_HEADING:
+      // Heading: 0-360° → 0-4095
+      if (imu.isReady()) {
+        outputValue = (uint16_t)((imu.getHeading() / 360.0f) * 4095.0f);
+      } else {
+        outputValue = 2048;  // Center if not ready
+      }
+      break;
+    case VAR_IMU_PITCH:
+      // Pitch: -90° to +90° → 0-4095 (2048 = level)
+      if (imu.isReady()) {
+        float pitch = constrain(imu.getPitch(), -90.0f, 90.0f);
+        outputValue = (uint16_t)(((pitch + 90.0f) / 180.0f) * 4095.0f);
+      } else {
+        outputValue = 2048;
+      }
+      break;
+    case VAR_IMU_ROLL:
+      // Roll: -180° to +180° → 0-4095 (2048 = level)
+      if (imu.isReady()) {
+        float roll = constrain(imu.getRoll(), -180.0f, 180.0f);
+        outputValue = (uint16_t)(((roll + 180.0f) / 360.0f) * 4095.0f);
+      } else {
+        outputValue = 2048;
+      }
+      break;
+      
+    // Radar sources - primary target (index 0)
+    case VAR_RADAR_X:
+      // X position: -200cm to +200cm → 0-4095 (2048 = center)
+      {
+        const RadarTarget& target = radar.getTarget(0);
+        if (target.valid) {
+          float x = constrain(target.x, -200.0f, 200.0f);
+          outputValue = (uint16_t)(((x + 200.0f) / 400.0f) * 4095.0f);
+        } else {
+          outputValue = 2048;  // Center if no target
+        }
+      }
+      break;
+    case VAR_RADAR_Y:
+      // Y distance: 0-500cm → 0-4095
+      {
+        const RadarTarget& target = radar.getTarget(0);
+        if (target.valid) {
+          float y = constrain(target.y, 0.0f, 500.0f);
+          outputValue = (uint16_t)((y / 500.0f) * 4095.0f);
+        } else {
+          outputValue = 0;  // No target = 0 distance
+        }
+      }
+      break;
   }
 }
 

nodegraph.h

@@ -19,7 +19,14 @@ enum VariableSource {
   VAR_FACE_Y,
   VAR_SINE,
   VAR_ANALOG,
-  VAR_SERVO_FEEDBACK
+  VAR_SERVO_FEEDBACK,
+  // IMU sources (BNO055)
+  VAR_IMU_HEADING,    // 0-360 degrees → 0-4095
+  VAR_IMU_PITCH,      // -90 to +90 degrees → 0-4095 (2048 = level)
+  VAR_IMU_ROLL,       // -180 to +180 degrees → 0-4095 (2048 = level)
+  // Radar sources (RD-03D) - primary target
+  VAR_RADAR_X,        // X position (angle) → 0-4095 (2048 = center)
+  VAR_RADAR_Y         // Y distance → 0-4095 (scaled 0-500cm)
 };
 
 

protocol.cpp

@@ -0,0 +1,209 @@
+#include "protocol.h"
+
+// ============================================================================
+// Global Buffers
+// ============================================================================
+
+uint8_t g_rxBuffer[MAX_PAYLOAD_SIZE + PACKET_HEADER_SIZE + PACKET_CRC_SIZE];
+uint16_t g_rxBufferLen = 0;
+
+// Parsed packet info
+static char s_rxTag[4];
+static uint16_t s_rxPayloadLen = 0;
+static uint16_t s_rxSeq = 0;
+static uint16_t s_rxPayloadOffset = 0;
+
+// Sequence counters (per-tag would be ideal, but global is simpler)
+static uint16_t s_txSeq = 0;
+
+// Receive state machine
+enum RxState { RX_SYNC0, RX_SYNC1, RX_HEADER, RX_PAYLOAD, RX_CRC };
+static RxState s_rxState = RX_SYNC0;
+static uint16_t s_rxIdx = 0;
+static uint16_t s_rxExpectedLen = 0;
+
+// ============================================================================
+// CRC16-CCITT (polynomial 0x1021, init 0xFFFF)
+// ============================================================================
+
+uint16_t crc16Update(uint16_t crc, const uint8_t* data, uint16_t len) {
+  for (uint16_t i = 0; i < len; i++) {
+    crc ^= (uint16_t)data[i] << 8;
+    for (uint8_t j = 0; j < 8; j++) {
+      if (crc & 0x8000) {
+        crc = (crc << 1) ^ 0x1021;
+      } else {
+        crc <<= 1;
+      }
+    }
+  }
+  return crc;
+}
+
+uint16_t crc16Compute(const uint8_t* data, uint16_t len) {
+  return crc16Update(0xFFFF, data, len);
+}
+
+// ============================================================================
+// Packet Sending
+// ============================================================================
+
+void sendPacket(const char tag[4], const uint8_t* payload, uint16_t len) {
+  // Build header
+  uint8_t header[PACKET_HEADER_SIZE];
+  header[0] = SYNC0;
+  header[1] = SYNC1;
+  header[2] = tag[0];
+  header[3] = tag[1];
+  header[4] = tag[2];
+  header[5] = tag[3];
+  header[6] = len & 0xFF;
+  header[7] = (len >> 8) & 0xFF;
+  header[8] = s_txSeq & 0xFF;
+  header[9] = (s_txSeq >> 8) & 0xFF;
+  
+  // Compute CRC over tag + len + seq + payload
+  uint16_t crc = 0xFFFF;
+  crc = crc16Update(crc, header + 2, 8);  // tag(4) + len(2) + seq(2)
+  if (len > 0 && payload != nullptr) {
+    crc = crc16Update(crc, payload, len);
+  }
+  
+  // Send
+  Serial.write(header, PACKET_HEADER_SIZE);
+  if (len > 0 && payload != nullptr) {
+    Serial.write(payload, len);
+  }
+  Serial.write(crc & 0xFF);
+  Serial.write((crc >> 8) & 0xFF);
+  
+  s_txSeq++;
+}
+
+void sendMessage(const String& msg) {
+  sendPacket(Tag::MSGE, (const uint8_t*)msg.c_str(), msg.length());
+}
+
+void sendAck(const char originalTag[4]) {
+  uint8_t payload[4];
+  memcpy(payload, originalTag, 4);
+  sendPacket(Tag::ACK, payload, 4);
+}
+
+void sendNack(const char originalTag[4], const String& reason) {
+  uint8_t payload[4 + 64];
+  memcpy(payload, originalTag, 4);
+  uint16_t len = 4;
+  
+  if (reason.length() > 0) {
+    uint16_t reasonLen = min((uint16_t)reason.length(), (uint16_t)60);
+    memcpy(payload + 4, reason.c_str(), reasonLen);
+    len += reasonLen;
+  }
+  
+  sendPacket(Tag::NACK, payload, len);
+}
+
+// ============================================================================
+// Packet Receiving
+// ============================================================================
+
+bool receivePacket() {
+  while (Serial.available()) {
+    uint8_t b = Serial.read();
+    
+    switch (s_rxState) {
+      case RX_SYNC0:
+        if (b == SYNC0) {
+          g_rxBuffer[0] = b;
+          s_rxState = RX_SYNC1;
+        }
+        break;
+        
+      case RX_SYNC1:
+        if (b == SYNC1) {
+          g_rxBuffer[1] = b;
+          s_rxIdx = 2;
+          s_rxState = RX_HEADER;
+        } else if (b == SYNC0) {
+          // Stay in SYNC1 state, might be repeated sync
+        } else {
+          s_rxState = RX_SYNC0;
+        }
+        break;
+        
+      case RX_HEADER:
+        g_rxBuffer[s_rxIdx++] = b;
+        if (s_rxIdx >= PACKET_HEADER_SIZE) {
+          // Parse header
+          memcpy(s_rxTag, g_rxBuffer + 2, 4);
+          s_rxPayloadLen = g_rxBuffer[6] | (g_rxBuffer[7] << 8);
+          s_rxSeq = g_rxBuffer[8] | (g_rxBuffer[9] << 8);
+          s_rxPayloadOffset = PACKET_HEADER_SIZE;
+          
+          // Validate length
+          if (s_rxPayloadLen > MAX_PAYLOAD_SIZE) {
+            Serial.println("Packet too large");
+            s_rxState = RX_SYNC0;
+            break;
+          }
+          
+          s_rxExpectedLen = PACKET_HEADER_SIZE + s_rxPayloadLen + PACKET_CRC_SIZE;
+          
+          if (s_rxPayloadLen == 0) {
+            s_rxState = RX_CRC;
+          } else {
+            s_rxState = RX_PAYLOAD;
+          }
+        }
+        break;
+        
+      case RX_PAYLOAD:
+        g_rxBuffer[s_rxIdx++] = b;
+        if (s_rxIdx >= PACKET_HEADER_SIZE + s_rxPayloadLen) {
+          s_rxState = RX_CRC;
+        }
+        break;
+        
+      case RX_CRC:
+        g_rxBuffer[s_rxIdx++] = b;
+        if (s_rxIdx >= s_rxExpectedLen) {
+          // Verify CRC
+          uint16_t receivedCrc = g_rxBuffer[s_rxIdx - 2] | (g_rxBuffer[s_rxIdx - 1] << 8);
+          uint16_t computedCrc = crc16Compute(g_rxBuffer + 2, s_rxPayloadLen + 8);
+          
+          s_rxState = RX_SYNC0;
+          g_rxBufferLen = s_rxIdx;
+          
+          if (receivedCrc == computedCrc) {
+            return true;  // Valid packet ready
+          } else {
+            Serial.println("CRC mismatch");
+          }
+        }
+        break;
+    }
+  }
+  
+  return false;
+}
+
+const char* getReceivedTag() {
+  return s_rxTag;
+}
+
+const uint8_t* getReceivedPayload() {
+  return g_rxBuffer + s_rxPayloadOffset;
+}
+
+uint16_t getReceivedPayloadLen() {
+  return s_rxPayloadLen;
+}
+
+uint16_t getReceivedSeq() {
+  return s_rxSeq;
+}
+
+bool tagMatches(const char* received, const char expected[4]) {
+  return memcmp(received, expected, 4) == 0;
+}

protocol.h

@@ -0,0 +1,118 @@
+#pragma once
+#include <Arduino.h>
+
+// ============================================================================
+// Protocol Constants
+// ============================================================================
+
+// Sync bytes (distinguishable from Feetech 0xFF 0xFF)
+constexpr uint8_t SYNC0 = 0xA5;
+constexpr uint8_t SYNC1 = 0x5A;
+
+// Packet structure:
+// [SYNC0][SYNC1][TAG 4 bytes][LENGTH 2 bytes][SEQ 2 bytes][PAYLOAD...][CRC16 2 bytes]
+// Total overhead: 12 bytes
+
+constexpr uint16_t MAX_PAYLOAD_SIZE = 6000;
+constexpr uint16_t PACKET_HEADER_SIZE = 10;  // sync(2) + tag(4) + len(2) + seq(2)
+constexpr uint16_t PACKET_CRC_SIZE = 2;
+
+// ============================================================================
+// Packet Tags (4 bytes each, human-readable)
+// ============================================================================
+
+namespace Tag {
+  // Identity & Config
+  constexpr char IDENT[4] = {'I','D','N','T'};  // Identity request/response
+  constexpr char CONFG[4] = {'C','O','N','F'};  // Config update
+  
+  // File Operations
+  constexpr char FLIST[4] = {'F','L','S','T'};  // File list
+  constexpr char FLOAD[4] = {'F','L','O','D'};  // File load
+  constexpr char FSAVE[4] = {'F','S','A','V'};  // File save
+  constexpr char FDELE[4] = {'F','D','E','L'};  // File delete
+  constexpr char FPLAY[4] = {'F','P','L','Y'};  // Play animation file
+  
+  // Motor Control
+  constexpr char MSET[4]  = {'M','S','E','T'};  // Set motor positions
+  constexpr char MPOS[4]  = {'M','P','O','S'};  // Motor position stream
+  constexpr char MSCAN[4] = {'M','S','C','N'};  // Scan for motors
+  constexpr char MWRIT[4] = {'M','W','R','T'};  // Write motor register
+  constexpr char MSTRM[4] = {'M','S','T','M'};  // Motor stream control
+  
+  // Sensors
+  constexpr char IMU[4]   = {'I','M','U','0'};  // IMU data (heading, roll, pitch)
+  constexpr char RADAR[4] = {'R','D','A','R'};  // Radar targets
+  
+  // System
+  constexpr char STATE[4] = {'S','T','A','T'};  // System state/heartbeat
+  constexpr char MSGE[4]  = {'M','S','G','E'};  // Log/debug message
+  constexpr char BOOT[4]  = {'B','O','O','T'};  // Enter bootloader
+  constexpr char ACK[4]   = {'A','C','K','!'};  // Acknowledge
+  constexpr char NACK[4]  = {'N','A','C','K'};  // Negative acknowledge
+}
+
+// ============================================================================
+// Play Modes (for FPLAY command)
+// ============================================================================
+
+enum PlayMode : uint8_t {
+  PLAY_IDLE   = 0x00,
+  PLAY_ONCE   = 0x01,
+  PLAY_LOOP   = 0x02,
+  PLAY_REPEAT = 0x03
+};
+
+// ============================================================================
+// Packet Buffer
+// ============================================================================
+
+extern uint8_t g_rxBuffer[MAX_PAYLOAD_SIZE + PACKET_HEADER_SIZE + PACKET_CRC_SIZE];
+extern uint16_t g_rxBufferLen;
+
+// ============================================================================
+// CRC16-CCITT
+// ============================================================================
+
+uint16_t crc16Update(uint16_t crc, const uint8_t* data, uint16_t len);
+uint16_t crc16Compute(const uint8_t* data, uint16_t len);
+
+// ============================================================================
+// Packet Sending
+// ============================================================================
+
+// Send a tagged packet with auto-incrementing sequence number
+void sendPacket(const char tag[4], const uint8_t* payload, uint16_t len);
+
+// Convenience: send string as MSGE packet
+void sendMessage(const String& msg);
+
+// Convenience: send ACK/NACK
+void sendAck(const char originalTag[4]);
+void sendNack(const char originalTag[4], const String& reason = "");
+
+// ============================================================================
+// Packet Receiving
+// ============================================================================
+
+// Packet receive state machine - call from loop()
+// Returns true when a complete valid packet is ready
+bool receivePacket();
+
+// Get received packet info (valid after receivePacket returns true)
+const char* getReceivedTag();
+const uint8_t* getReceivedPayload();
+uint16_t getReceivedPayloadLen();
+uint16_t getReceivedSeq();
+
+// Check if received tag matches
+bool tagMatches(const char* received, const char expected[4]);
+
+// ============================================================================
+// Utility
+// ============================================================================
+
+// Compare tags
+inline bool tagsEqual(const char a[4], const char b[4]) {
+  return memcmp(a, b, 4) == 0;
+}

sensors.cpp

@@ -0,0 +1,271 @@
+#include "sensors.h"
+#include "protocol.h"
+
+// ============================================================================
+// Global Instances
+// ============================================================================
+
+Radar radar;
+IMU imu;
+SensorManager sensors;
+
+// ============================================================================
+// Radar Implementation
+// ============================================================================
+
+static const uint8_t RADAR_HEADER[] = {0xAA, 0xFF, 0x03, 0x00};
+static const uint8_t RADAR_FOOTER[] = {0x55, 0xCC};
+constexpr float RADAR_DISTANCE_SCALE = 0.1f;  // Raw mm to cm
+constexpr float RADAR_MIN_VALID_DIST = 30.0f; // Minimum valid distance in cm
+
+int16_t Radar::decodeSignMag(uint16_t raw) {
+  int16_t magnitude = raw & 0x7FFF;
+  return (raw & 0x8000) ? magnitude : -magnitude;
+}
+
+void Radar::init() {
+  Serial2.begin(RADAR_BAUD, SERIAL_8N1, SensorPins::RADAR_RX, SensorPins::RADAR_TX);
+}
+
+bool Radar::update() {
+  bool newData = false;
+  
+  while (Serial2.available()) {
+    uint8_t b = Serial2.read();
+    
+    if (!inFrame) {
+      // Looking for header
+      if (b == RADAR_HEADER[headerMatch]) {
+        rxBuf[headerMatch] = b;
+        headerMatch++;
+        if (headerMatch == 4) {
+          inFrame = true;
+          bufIdx = 4;
+          headerMatch = 0;
+        }
+      } else if (b == RADAR_HEADER[0]) {
+        headerMatch = 1;
+        rxBuf[0] = b;
+      } else {
+        headerMatch = 0;
+      }
+      continue;
+    }
+    
+    // In frame - collect bytes
+    if (bufIdx < sizeof(rxBuf)) {
+      rxBuf[bufIdx++] = b;
+    }
+    
+    // Check for footer
+    if (bufIdx >= 6 && rxBuf[bufIdx - 2] == RADAR_FOOTER[0] && rxBuf[bufIdx - 1] == RADAR_FOOTER[1]) {
+      parseFrame();
+      newData = true;
+      inFrame = false;
+      bufIdx = 0;
+    }
+    
+    // Overflow protection
+    if (bufIdx >= sizeof(rxBuf)) {
+      inFrame = false;
+      bufIdx = 0;
+    }
+  }
+  
+  return newData;
+}
+
+void Radar::parseFrame() {
+  for (int i = 0; i < RADAR_MAX_TARGETS; i++) {
+    int offset = 4 + (i * 6);
+    
+    uint16_t x_raw = rxBuf[offset] | (rxBuf[offset + 1] << 8);
+    uint16_t y_raw = rxBuf[offset + 2] | (rxBuf[offset + 3] << 8);
+    uint16_t spd_raw = rxBuf[offset + 4] | (rxBuf[offset + 5] << 8);
+    
+    targets[i].x = decodeSignMag(x_raw) * RADAR_DISTANCE_SCALE;
+    targets[i].y = (int16_t)(y_raw - 0x8000) * RADAR_DISTANCE_SCALE;
+    targets[i].speed = decodeSignMag(spd_raw) * RADAR_DISTANCE_SCALE;
+    
+    targets[i].valid = (y_raw != 0) && (y_raw != 0x8000) && (targets[i].y >= RADAR_MIN_VALID_DIST);
+  }
+}
+
+const RadarTarget& Radar::getTarget(uint8_t index) const {
+  if (index >= RADAR_MAX_TARGETS) index = 0;
+  return targets[index];
+}
+
+uint8_t Radar::getTargetCount() const {
+  uint8_t count = 0;
+  for (int i = 0; i < RADAR_MAX_TARGETS; i++) {
+    if (targets[i].valid) count++;
+  }
+  return count;
+}
+
+uint16_t Radar::packPayload(uint8_t* buffer) const {
+  // Format: count(1) + [valid(1), x(2), y(2), speed(2)] * 3 = 22 bytes
+  buffer[0] = getTargetCount();
+  uint16_t offset = 1;
+  
+  for (int i = 0; i < RADAR_MAX_TARGETS; i++) {
+    buffer[offset++] = targets[i].valid ? 1 : 0;
+    
+    int16_t x = (int16_t)(targets[i].x * 10);  // cm * 10 for precision
+    int16_t y = (int16_t)(targets[i].y * 10);
+    int16_t spd = (int16_t)(targets[i].speed * 10);
+    
+    buffer[offset++] = x & 0xFF;
+    buffer[offset++] = (x >> 8) & 0xFF;
+    buffer[offset++] = y & 0xFF;
+    buffer[offset++] = (y >> 8) & 0xFF;
+    buffer[offset++] = spd & 0xFF;
+    buffer[offset++] = (spd >> 8) & 0xFF;
+  }
+  
+  return offset;
+}
+
+// ============================================================================
+// IMU Implementation
+// ============================================================================
+
+IMU::IMU(uint8_t addr) : addr(addr) {}
+
+bool IMU::init() {
+  Wire.begin(SensorPins::IMU_SDA, SensorPins::IMU_SCL);
+  delay(100);
+  
+  uint8_t id = read8(0x00);  // CHIP_ID register
+  if (id != 0xA0) {
+    ready = false;
+    return false;
+  }
+  
+  // Enter config mode
+  write8(0x3D, 0x00);
+  delay(25);
+  
+  // Set NDOF fusion mode
+  write8(0x3D, 0x0C);
+  delay(25);
+  
+  ready = true;
+  return true;
+}
+
+bool IMU::update() {
+  if (!ready) return false;
+  
+  Wire.beginTransmission(addr);
+  Wire.write(0x1A);  // Euler angles start register
+  Wire.endTransmission();
+  
+  Wire.requestFrom(addr, (uint8_t)6);
+  if (Wire.available() < 6) return false;
+  
+  int16_t rawHeading = Wire.read() | (Wire.read() << 8);
+  int16_t rawPitch = Wire.read() | (Wire.read() << 8);
+  int16_t rawRoll = Wire.read() | (Wire.read() << 8);
+  
+  // Convert from 1/16 degree units
+  heading = rawHeading / 16.0f;
+  roll = -(rawRoll / 16.0f);  // Inverted so right roll is positive
+  pitch = rawPitch / 16.0f;
+  
+  return true;
+}
+
+uint16_t IMU::packPayload(uint8_t* buffer) const {
+  // Format: heading(2) + roll(2) + pitch(2) as int16 * 100
+  int16_t h = (int16_t)(heading * 100.0f);
+  int16_t r = (int16_t)(roll * 100.0f);
+  int16_t p = (int16_t)(pitch * 100.0f);
+  
+  buffer[0] = h & 0xFF;
+  buffer[1] = (h >> 8) & 0xFF;
+  buffer[2] = r & 0xFF;
+  buffer[3] = (r >> 8) & 0xFF;
+  buffer[4] = p & 0xFF;
+  buffer[5] = (p >> 8) & 0xFF;
+  
+  return 6;
+}
+
+void IMU::write8(uint8_t reg, uint8_t value) {
+  Wire.beginTransmission(addr);
+  Wire.write(reg);
+  Wire.write(value);
+  Wire.endTransmission();
+}
+
+uint8_t IMU::read8(uint8_t reg) {
+  Wire.beginTransmission(addr);
+  Wire.write(reg);
+  Wire.endTransmission();
+  Wire.requestFrom(addr, (uint8_t)1);
+  return Wire.available() ? Wire.read() : 0xFF;
+}
+
+// ============================================================================
+// Sensor Manager Implementation
+// ============================================================================
+
+void SensorManager::init() {
+  radar.init();
+  
+  if (imu.init()) {
+    Serial.println("[Sensors] IMU initialized");
+  } else {
+    Serial.println("[Sensors] IMU not detected");
+  }
+  
+  Serial.println("[Sensors] Radar initialized");
+}
+
+void SensorManager::update() {
+  // Update sensors
+  radar.update();
+  if (imu.isReady()) {
+    imu.update();
+  }
+  
+  // Handle streaming
+  unsigned long now = millis();
+  
+  if (imuStreamEnabled && imu.isReady() && (now - lastIMUSend >= imuInterval)) {
+    sendIMUPacket();
+    lastIMUSend = now;
+  }
+  
+  if (radarStreamEnabled && (now - lastRadarSend >= radarInterval)) {
+    sendRadarPacket();
+    lastRadarSend = now;
+  }
+}
+
+void SensorManager::enableIMUStream(bool enable, uint16_t intervalMs) {
+  imuStreamEnabled = enable;
+  imuInterval = intervalMs;
+  lastIMUSend = millis();
+}
+
+void SensorManager::enableRadarStream(bool enable, uint16_t intervalMs) {
+  radarStreamEnabled = enable;
+  radarInterval = intervalMs;
+  lastRadarSend = millis();
+}
+
+void SensorManager::sendIMUPacket() {
+  uint8_t payload[6];
+  uint16_t len = imu.packPayload(payload);
+  sendPacket(Tag::IMU, payload, len);
+}
+
+void SensorManager::sendRadarPacket() {
+  uint8_t payload[32];
+  uint16_t len = radar.packPayload(payload);
+  sendPacket(Tag::RADAR, payload, len);
+}
+

sensors.h

@@ -0,0 +1,121 @@
+#pragma once
+#include <Arduino.h>
+#include <Wire.h>
+
+// ============================================================================
+// Pin Configuration
+// ============================================================================
+
+namespace SensorPins {
+  // Radar (RD-03D) on Serial2
+  constexpr int RADAR_RX = 4;
+  constexpr int RADAR_TX = 5;
+  
+  // IMU (BNO055) on I2C
+  constexpr int IMU_SDA = 8;
+  constexpr int IMU_SCL = 9;
+}
+
+// ============================================================================
+// Radar - RD-03D mmWave Sensor
+// ============================================================================
+
+constexpr int RADAR_MAX_TARGETS = 3;
+constexpr uint32_t RADAR_BAUD = 256000;
+
+struct RadarTarget {
+  float x;        // X position in cm (negative=left, positive=right)
+  float y;        // Y distance in cm (forward)
+  float speed;    // Speed in cm/s
+  bool valid;     // Target is valid
+};
+
+class Radar {
+public:
+  void init();
+  bool update();  // Returns true if new data parsed
+  
+  const RadarTarget& getTarget(uint8_t index) const;
+  uint8_t getTargetCount() const;
+  
+  // Pack all targets into a payload buffer, returns length
+  uint16_t packPayload(uint8_t* buffer) const;
+  
+private:
+  RadarTarget targets[RADAR_MAX_TARGETS] = {};
+  uint8_t rxBuf[64];
+  uint8_t bufIdx = 0;
+  uint8_t headerMatch = 0;
+  bool inFrame = false;
+  
+  void parseFrame();
+  static int16_t decodeSignMag(uint16_t raw);
+};
+
+// ============================================================================
+// IMU - BNO055 9-DOF Sensor
+// ============================================================================
+
+class IMU {
+public:
+  IMU(uint8_t addr = 0x29);
+  
+  bool init();
+  bool update();  // Read latest values
+  
+  // Get euler angles in degrees
+  float getHeading() const { return heading; }
+  float getRoll() const { return roll; }
+  float getPitch() const { return pitch; }
+  
+  bool isReady() const { return ready; }
+  
+  // Pack into payload buffer (6 bytes: h, r, p as int16 * 100)
+  uint16_t packPayload(uint8_t* buffer) const;
+  
+private:
+  uint8_t addr;
+  bool ready = false;
+  float heading = 0, roll = 0, pitch = 0;
+  
+  void write8(uint8_t reg, uint8_t value);
+  uint8_t read8(uint8_t reg);
+};
+
+// ============================================================================
+// Global Instances
+// ============================================================================
+
+extern Radar radar;
+extern IMU imu;
+
+// ============================================================================
+// Sensor Manager
+// ============================================================================
+
+class SensorManager {
+public:
+  void init();
+  void update();
+  
+  // Streaming control
+  void enableIMUStream(bool enable, uint16_t intervalMs = 100);
+  void enableRadarStream(bool enable, uint16_t intervalMs = 100);
+  
+  bool isIMUStreamEnabled() const { return imuStreamEnabled; }
+  bool isRadarStreamEnabled() const { return radarStreamEnabled; }
+  
+private:
+  bool imuStreamEnabled = false;
+  bool radarStreamEnabled = false;
+  uint16_t imuInterval = 100;
+  uint16_t radarInterval = 100;
+  unsigned long lastIMUSend = 0;
+  unsigned long lastRadarSend = 0;
+  
+  void sendIMUPacket();
+  void sendRadarPacket();
+};
+
+extern SensorManager sensors;
+