motor assist works on feet now. a little over sensitive

ANIM_V2

@@ -0,0 +1,62 @@
+# Animation File Format Version 2 - Encoding Specification
+
+## File Structure
+
+The file consists of three sections in order:
+
+1. **Filename Block** (optional, for serial transmission)
+2. **Header Block** (16 bytes)
+3. **Frame Data Block** (variable size)
+
+## Encoding Details
+
+### 1. Filename Block
+```
+[filename_length: 2 bytes, uint16_t, little-endian]
+[filename_bytes: N bytes, UTF-8 string]
+```
+
+### 2. Header Block (16 bytes total)
+```
+[0-3]    "ANIM" (4 bytes, ASCII)
+[4-5]    frameCount (2 bytes, uint16_t, little-endian)
+[6]      version (1 byte, uint8_t) = 2
+[7]      frameRate (1 byte, uint8_t) = FPS
+[8-15]   reserved (8 bytes, all zeros)
+```
+
+### 3. Frame Data Block
+For each frame (0 to frameCount-1), all motors are stored in the same order:
+
+```
+For each frame:
+  For each motor (in consistent order):
+    [motor_id: 1 byte, uint8_t]
+    [position: 2 bytes, uint16_t, little-endian, range 0-4095]
+```
+
+**Important**: 
+- All frames contain the same motors in the same order
+- Motor count = (Frame Data Block size) / (frameCount * 3)
+- Each motor record is exactly 3 bytes: 1 byte ID + 2 bytes position
+
+## Example File Layout
+
+For a file with 100 frames and 20 motors:
+
+```
+[0-1]        Filename length (2 bytes)
+[2-N]        Filename (N bytes)
+[N+0-N+3]    "ANIM" (4 bytes)
+[N+4-N+5]    100 (frameCount, 2 bytes)
+[N+6]        2 (version, 1 byte)
+[N+7]        24 (frameRate, 1 byte)
+[N+8-N+15]   Reserved (8 bytes)
+[N+16+]      Frame data:
+              Frame 0: [motor0_id][motor0_pos][motor1_id][motor1_pos]...[motor19_id][motor19_pos]
+              Frame 1: [motor0_id][motor0_pos][motor1_id][motor1_pos]...[motor19_id][motor19_pos]
+              ...
+              Frame 99: [motor0_id][motor0_pos][motor1_id][motor1_pos]...[motor19_id][motor19_pos]
+```
+
+Total frame data size = 100 frames × 20 motors × 3 bytes = 6,000 bytes

PROTOCOL_MIGRATION.md

@@ -0,0 +1,545 @@
+# 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 |
+| `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*
+
+---
+
+### 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 (radar 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")
+
+---
+
+### 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
+

animation.cpp

@@ -81,6 +81,25 @@ uint16_t Animation::getMotorPosition(uint8_t motorID, uint16_t timeCS) {
 
 void Animation::clear() {
   //memset(data, 0, sizeof(data));
+  frameData.clear();
+}
+
+void Animation::setFrameData(uint16_t frameIndex, const std::vector<MotorPosition>& motors) {
+  if (frameIndex >= frameData.size()) {
+    frameData.resize(frameIndex + 1);
+  }
+  frameData[frameIndex] = motors;
+}
+
+const std::vector<MotorPosition>* Animation::getFrameData(uint16_t frameIndex) const {
+  if (frameIndex >= frameData.size()) {
+    return nullptr;
+  }
+  return &frameData[frameIndex];
+}
+
+void Animation::clearFrameData() {
+  frameData.clear();
 }
 
 // uint16_t* Animation::getRawData() {
@@ -105,20 +124,32 @@ bool Animation::saveToFile(const char* filename) {
 
   file.write((uint8_t*)&header, sizeof(header));
 
-  uint16_t curveCount = 0;
+  if (header.version == 2) {
-  for (const auto& [motorID, segments] : curves) {
+    // Version 2: Write frame data
-    curveCount += segments.size();
+    // For each frame, write all motor positions
-  }
+    for (uint16_t frameIndex = 0; frameIndex < frameData.size() && frameIndex < header.frameCount; frameIndex++) {
-  file.write((uint8_t*)&curveCount, sizeof(curveCount));
+      const auto& frame = frameData[frameIndex];
-  for (const auto& [motorID, segments] : curves) {
+      for (const auto& motorPos : frame) {
-    for (const CurveSegment& seg : segments) {
+        file.write((uint8_t*)&motorPos, sizeof(MotorPosition));
-      file.write((uint8_t*)&seg, sizeof(CurveSegment));
+      }
+    }
+  } else {
+    // Version 1: Write curves and node graph
+    uint16_t curveCount = 0;
+    for (const auto& [motorID, segments] : curves) {
+      curveCount += segments.size();
+    }
+    file.write((uint8_t*)&curveCount, sizeof(curveCount));
+    for (const auto& [motorID, segments] : curves) {
+      for (const CurveSegment& seg : segments) {
+        file.write((uint8_t*)&seg, sizeof(CurveSegment));
+      }
     }
-  }
 
-  // ✅ Write serialized node graph
+    // ✅ Write serialized node graph
-  std::vector<uint8_t> graphData = nodeGraph.serialize();
+    std::vector<uint8_t> graphData = nodeGraph.serialize();
-  file.write(graphData.data(), graphData.size());
+    file.write(graphData.data(), graphData.size());
+  }
 
   file.close();
   return true;
@@ -136,46 +167,87 @@ bool Animation::loadFromFile(const char* filename) {
     return false;
   }
 
-  if (strncmp(tempHeader.magic, "ANIM", 4) != 0 || tempHeader.version != 1) {
+  if (strncmp(tempHeader.magic, "ANIM", 4) != 0) {
+    file.close();
+    return false;
+  }
+
+  if (tempHeader.version != 1 && tempHeader.version != 2) {
     file.close();
     return false;
   }
 
   header = tempHeader;
 
-  // Read curve count
+  if (header.version == 2) {
-  uint16_t curveCount;
+    // Version 2: Read frame data
-  if (file.read((uint8_t*)&curveCount, sizeof(curveCount)) != sizeof(curveCount)) {
+    clearFrameData();
-    file.close();
+    frameData.reserve(header.frameCount);
-    return false;
-  }
     
-  clearAllCurves();
+    // Calculate motor count from file size
+    size_t fileSize = file.size();
+    size_t headerSize = sizeof(AnimationHeader);
+    size_t frameDataSize = fileSize - headerSize;
+    size_t frameSize = frameDataSize / header.frameCount;  // bytes per frame
+    uint16_t motorCount = frameSize / sizeof(MotorPosition);  // motors per frame
     
-  // Read curve segments
+    if (frameSize % sizeof(MotorPosition) != 0) {
-  for (uint16_t i = 0; i < curveCount; i++) {
-    CurveSegment seg;
-    if (file.read((uint8_t*)&seg, sizeof(CurveSegment)) != sizeof(CurveSegment)) {
       file.close();
-      return false;
+      return false;  // Invalid frame data size
     }
-    curves[seg.motorID].push_back(seg);
-  }
     
-  // ✅ Read remaining bytes into buffer
+    // Read all frames
-  size_t remaining = file.available();
+    for (uint16_t frameIndex = 0; frameIndex < header.frameCount; frameIndex++) {
-  if (remaining > 0) {
+      std::vector<MotorPosition> frame;
-    std::vector<uint8_t> buffer(remaining);
+      frame.reserve(motorCount);
-    if (file.read(buffer.data(), remaining) != remaining) {
+      
+      for (uint16_t motorIndex = 0; motorIndex < motorCount; motorIndex++) {
+        MotorPosition motorPos;
+        if (file.read((uint8_t*)&motorPos, sizeof(MotorPosition)) != sizeof(MotorPosition)) {
+          file.close();
+          return false;
+        }
+        frame.push_back(motorPos);
+      }
+      
+      frameData.push_back(frame);
+    }
+  } else {
+    // Version 1: Read curves and node graph
+    // Read curve count
+    uint16_t curveCount;
+    if (file.read((uint8_t*)&curveCount, sizeof(curveCount)) != sizeof(curveCount)) {
       file.close();
       return false;
     }
 
-    // ✅ Load node graph from buffer
+    clearAllCurves();
-    nodeGraph.nodes.clear();
+
-    nodeGraph.connections.clear();
+    // Read curve segments
-    loadNodeGraph(buffer.data(), buffer.size(), nodeGraph);
+    for (uint16_t i = 0; i < curveCount; i++) {
-    nodeGraph.bindAnimationContext(this);
+      CurveSegment seg;
+      if (file.read((uint8_t*)&seg, sizeof(CurveSegment)) != sizeof(CurveSegment)) {
+        file.close();
+        return false;
+      }
+      curves[seg.motorID].push_back(seg);
+    }
+
+    // ✅ Read remaining bytes into buffer
+    size_t remaining = file.available();
+    if (remaining > 0) {
+      std::vector<uint8_t> buffer(remaining);
+      if (file.read(buffer.data(), remaining) != remaining) {
+        file.close();
+        return false;
+      }
+
+      // ✅ Load node graph from buffer
+      nodeGraph.nodes.clear();
+      nodeGraph.connections.clear();
+      loadNodeGraph(buffer.data(), buffer.size(), nodeGraph);
+      nodeGraph.bindAnimationContext(this);
+    }
   }
 
   file.close();
@@ -211,6 +283,54 @@ String Animation::printCurves() {
   return output;
 }
 
+String Animation::printAnim() {
+  String output = "ANIMATION INFO\n";
+  output += "==============\n";
+  output += "Version: " + String(header.version) + "\n";
+  output += "Frame Count: " + String(header.frameCount) + "\n";
+  output += "Frame Rate: " + String(header.frameRate) + " fps\n";
+  
+  if (header.frameRate > 0) {
+    float duration = (float)header.frameCount / (float)header.frameRate;
+    output += "Duration: " + String(duration, 2) + " seconds\n";
+  }
+  
+  output += "Active: " + String(isActive() ? "Yes" : "No") + "\n";
+  
+  if (header.version == 1) {
+    // Version 1: curves and node graph
+    uint16_t curveCount = 0;
+    for (const auto& [motorID, segments] : curves) {
+      curveCount += segments.size();
+    }
+    output += "Curve Segments: " + String(curveCount) + "\n";
+    output += "Motors with Curves: " + String(curves.size()) + "\n";
+    output += "Node Graph Nodes: " + String(nodeGraph.nodes.size()) + "\n";
+    output += "Node Graph Connections: " + String(nodeGraph.connections.size()) + "\n";
+  } else if (header.version == 2) {
+    // Version 2: frame data
+    if (!frameData.empty()) {
+      uint16_t motorCount = frameData[0].size();
+      output += "Motors per Frame: " + String(motorCount) + "\n";
+      output += "Frames Stored: " + String(frameData.size()) + "\n";
+      
+      // Show motor IDs from first frame
+      if (!frameData[0].empty()) {
+        output += "Motor IDs: ";
+        for (size_t i = 0; i < frameData[0].size(); i++) {
+          if (i > 0) output += ", ";
+          output += String(frameData[0][i].motorID);
+        }
+        output += "\n";
+      }
+    } else {
+      output += "Frame Data: Empty\n";
+    }
+  }
+  
+  return output;
+}
+
 
 
 

animation.h

@@ -4,6 +4,7 @@
 #include "FS.h"
 #include "FFat.h"
 #include <unordered_map>
+#include <vector>
 #include "nodegraph.h"
 
 
@@ -34,6 +35,12 @@ struct __attribute__((packed)) CurveSegment {
   int16_t endPointY;
 };
 
+// Version 2 frame data: motor ID + position pair
+struct __attribute__((packed)) MotorPosition {
+  uint8_t motorID;
+  uint16_t position;  // 0-4095
+};
+
 
 
 
@@ -51,8 +58,14 @@ public:
   void clearCurves(uint8_t motorID);
   void clearAllCurves();
   String printCurves();
+  String printAnim();
   uint16_t getMotorPosition(uint8_t motorID, uint16_t timeCS);
   
+  // Version 2 frame data methods
+  void setFrameData(uint16_t frameIndex, const std::vector<MotorPosition>& motors);
+  const std::vector<MotorPosition>* getFrameData(uint16_t frameIndex) const;
+  void clearFrameData();
+  
   void clear();
   //uint16_t* getRawData();  // Optional: for bulk access
   //size_t getSize() const;
@@ -68,7 +81,8 @@ public:
 
 private:
   //uint16_t data[MAX_FRAMES][NUM_CHANNELS];
-  std::unordered_map<uint8_t, std::vector<CurveSegment>> curves;
+  std::unordered_map<uint8_t, std::vector<CurveSegment>> curves;  // Version 1: curves
+  std::vector<std::vector<MotorPosition>> frameData;  // Version 2: raw frame data
   bool active = false;
 };
 

behaviors.cpp

@@ -0,0 +1,700 @@
+#include "behaviors.h"
+#include <algorithm>
+
+// ============================================================================
+// Base Behavior Implementation
+// ============================================================================
+
+Behavior::Behavior() {
+  controlledMotors.clear();
+}
+
+void Behavior::addMotor(uint8_t motorID) {
+  // Check if motor already in list
+  for (uint8_t id : controlledMotors) {
+    if (id == motorID) {
+      return;  // Already added
+    }
+  }
+  controlledMotors.push_back(motorID);
+}
+
+void Behavior::removeMotor(uint8_t motorID) {
+  controlledMotors.erase(
+    std::remove(controlledMotors.begin(), controlledMotors.end(), motorID),
+    controlledMotors.end()
+  );
+}
+
+void Behavior::clearMotors() {
+  controlledMotors.clear();
+}
+
+// ============================================================================
+// Focus Behavior Implementation
+// ============================================================================
+
+FocusBehavior::FocusBehavior() {
+  isActive = false;
+  eyePosition = EYE_POSITION_CENTER;
+  neckPosition = NECK_POSITION_CENTER;
+  targetEyePosition = EYE_POSITION_CENTER;
+  targetNeckPosition = NECK_POSITION_CENTER;
+  targetDetectedTime = 0;
+  neckStartTime = 0;
+  neckRotating = false;
+  
+  // Add motors 14, 15, and 27 to controlled list
+  addMotor(FOCUS_MOTOR_1);
+  addMotor(FOCUS_MOTOR_2);
+  addMotor(NECK_MOTOR);
+}
+
+bool FocusBehavior::update() {
+  // Check radar for valid targets
+  uint8_t targetCount = radar.getTargetCount();
+  unsigned long now = millis();
+  
+  if (targetCount == 0 || !radar.getTarget(0).valid) {
+    // No target - return to center
+    isActive = false;
+    targetEyePosition = EYE_POSITION_CENTER;
+    targetNeckPosition = NECK_POSITION_CENTER;
+    targetDetectedTime = 0;
+    neckRotating = false;
+    
+    // Smoothly interpolate eyes to center
+    eyePosition = lerp(eyePosition, EYE_POSITION_CENTER, INTERPOLATION_SPEED);
+    
+    // Keep neck at center (no movement)
+    neckPosition = NECK_POSITION_CENTER;
+    
+    return false;
+  }
+
+  // Get first valid target
+  const RadarTarget& target = radar.getTarget(0);
+  
+  if (!target.valid) {
+    isActive = false;
+    targetEyePosition = EYE_POSITION_CENTER;
+    targetNeckPosition = NECK_POSITION_CENTER;
+    targetDetectedTime = 0;
+    neckRotating = false;
+    return false;
+  }
+
+  // Active tracking - calculate target positions from radar angle
+  isActive = true;
+  uint16_t targetEyePos = calculateEyePositionFromRadarAngle(target.angle);
+  
+  // Eyes track immediately
+  targetEyePosition = targetEyePos;
+  
+  // Neck disabled for now - keep it centered
+  targetNeckPosition = NECK_POSITION_CENTER;
+  neckRotating = false;
+  
+  // Smoothly interpolate eye position toward target
+  eyePosition = lerp(eyePosition, targetEyePosition, INTERPOLATION_SPEED);
+  
+  // Keep neck at center (no movement)
+  neckPosition = NECK_POSITION_CENTER;
+  
+  return true;
+}
+
+bool FocusBehavior::getMotorPosition(uint8_t motorID, uint16_t& position) {
+  // Provide position for eyes (motors 14 and 15)
+  if (motorID == FOCUS_MOTOR_1 || motorID == FOCUS_MOTOR_2) {
+    position = eyePosition;
+    return true;
+  }
+  
+  // Provide position for neck (motor 27)
+  if (motorID == NECK_MOTOR) {
+    position = neckPosition;
+    return true;
+  }
+  
+  return false;
+}
+
+uint16_t FocusBehavior::calculateEyePositionFromRadarAngle(float radarAngle) {
+  // Calculate eye motor position from radar angle (in degrees)
+  // Angle range: -50 to +50 degrees, mapped to full eye range (1700-2500, center 2200)
+  
+  constexpr float ANGLE_MIN = -50.0f;
+  constexpr float ANGLE_MAX = 50.0f;
+  
+  // Clamp angle to -50 to +50 range
+  if (radarAngle < ANGLE_MIN) radarAngle = ANGLE_MIN;
+  if (radarAngle > ANGLE_MAX) radarAngle = ANGLE_MAX;
+  
+  // Normalize angle to -1.0 to 1.0 range
+  float normalized = radarAngle / 50.0f;
+  
+  // Calculate range from center in each direction
+  // Left range: 2200 - 1700 = 500, Right range: 2500 - 2200 = 300
+  float rangeLeft = (float)(EYE_POSITION_CENTER - EYE_POSITION_MIN);   // 500
+  float rangeRight = (float)(EYE_POSITION_MAX - EYE_POSITION_CENTER);  // 300
+  
+  // Use different ranges for left (negative) and right (positive) to use full range
+  float positionFloat;
+  if (normalized < 0.0f) {
+    // Negative angle: use left range (500 units)
+    positionFloat = (float)EYE_POSITION_CENTER + (normalized * rangeLeft);
+  } else {
+    // Positive angle: use right range (300 units)
+    positionFloat = (float)EYE_POSITION_CENTER + (normalized * rangeRight);
+  }
+  
+  // Convert to int16_t first to handle negative values, then clamp
+  int16_t position = (int16_t)positionFloat;
+
+  // Clamp to valid range (1700 to 2500)
+  if (position < (int16_t)EYE_POSITION_MIN) position = (int16_t)EYE_POSITION_MIN;
+  if (position > (int16_t)EYE_POSITION_MAX) position = (int16_t)EYE_POSITION_MAX;
+
+  return (uint16_t)position;
+}
+
+uint16_t FocusBehavior::calculateNeckPositionFromRadarAngle(float radarAngle) {
+  // Calculate neck motor position from radar angle (in degrees)
+  // Angle range: approximately -45 to +45 degrees (typical radar FOV)
+  // Map to neck motor position range: 1000 to 3000 (center 2000)
+  // NOTE: Rotation is inverted for neck motor
+  
+  // Clamp angle to reasonable range (can extend later if needed)
+  constexpr float ANGLE_MIN = -45.0f;
+  constexpr float ANGLE_MAX = 45.0f;
+  
+  if (radarAngle < ANGLE_MIN) radarAngle = ANGLE_MIN;
+  if (radarAngle > ANGLE_MAX) radarAngle = ANGLE_MAX;
+  
+  // Normalize angle to -1.0 to 1.0 range, then invert for neck motor
+  float normalizedAngle = -(radarAngle / ANGLE_MAX);
+  
+  // Calculate range from center
+  float rangeLeft = NECK_POSITION_CENTER - NECK_POSITION_MIN;   // 1000
+  float rangeRight = NECK_POSITION_MAX - NECK_POSITION_CENTER;  // 1000
+  
+  uint16_t position;
+  if (normalizedAngle < 0.0f) {
+    // Left side: use left range
+    position = NECK_POSITION_CENTER + (uint16_t)(normalizedAngle * rangeLeft);
+  } else {
+    // Right side: use right range
+    position = NECK_POSITION_CENTER + (uint16_t)(normalizedAngle * rangeRight);
+  }
+
+  // Clamp to valid range
+  if (position < NECK_POSITION_MIN) position = NECK_POSITION_MIN;
+  if (position > NECK_POSITION_MAX) position = NECK_POSITION_MAX;
+
+  return position;
+}
+
+uint16_t FocusBehavior::lerp(uint16_t current, uint16_t target, float t) {
+  // Linear interpolation with clamping to prevent overshoot
+  int16_t diff = (int16_t)target - (int16_t)current;
+  int16_t delta = (int16_t)(diff * t);
+  
+  // If difference is very small, snap to target
+  if (abs(diff) < 2) {
+    return target;
+  }
+  
+  return (uint16_t)(current + delta);
+}
+
+// ============================================================================
+// Idle Behavior Implementation
+// ============================================================================
+
+IdleBehavior::IdleBehavior() {
+  startTime = millis();
+  
+  // Initialize all motor positions to center
+  for (int i = 0; i < 256; i++) {
+    motorPositions[i] = POSITION_CENTER;
+  }
+}
+
+void IdleBehavior::initMotors(const std::vector<uint8_t>& motorIDs) {
+  clearMotors();
+  for (uint8_t id : motorIDs) {
+    addMotor(id);
+    motorPositions[id] = POSITION_CENTER;
+  }
+}
+
+bool IdleBehavior::update() {
+  unsigned long now = millis();
+  float timeOffset = (float)(now - startTime) * NOISE_SPEED;
+  
+  // Update position for each controlled motor using perlin noise
+  for (uint8_t motorID : controlledMotors) {
+    // Use motor ID as seed offset for variety between motors
+    uint16_t seed = motorID * MOTOR_SEED_OFFSET;
+    
+    // Get perlin noise value (-1 to 1 range approximately)
+    float noiseValue = perlin1D_octave(seed, timeOffset, 3, 0.5f);
+    
+    // Map noise to position range: center ± NOISE_RANGE
+    // Perlin noise typically returns values in roughly -1 to 1 range
+    int16_t offset = (int16_t)(noiseValue * (float)NOISE_RANGE);
+    
+    // Calculate final position
+    int16_t position = (int16_t)POSITION_CENTER + offset;
+    
+    // Clamp to valid servo range
+    if (position < 1547) position = 1547;  // center - 500
+    if (position > 2547) position = 2547;  // center + 500
+    
+    motorPositions[motorID] = (uint16_t)position;
+  }
+  
+  // Idle behavior is always active (but low priority)
+  return true;
+}
+
+bool IdleBehavior::getMotorPosition(uint8_t motorID, uint16_t& position) {
+  // Check if we control this motor
+  for (uint8_t id : controlledMotors) {
+    if (id == motorID) {
+      position = motorPositions[motorID];
+      return true;
+    }
+  }
+  return false;
+}
+
+// ============================================================================
+// Viseme Behavior Implementation
+// ============================================================================
+
+VisemeBehavior::VisemeBehavior() {
+  isActive = false;
+  lastTriggerTime = 0;
+  nextVisemeID = 0;
+  currentPositions.clear();
+  visemes.clear();
+}
+
+Viseme* VisemeBehavior::findViseme(uint8_t id) {
+  for (Viseme& v : visemes) {
+    if (v.id == id) {
+      return &v;
+    }
+  }
+  return nullptr;
+}
+
+uint8_t VisemeBehavior::addViseme(const char* label) {
+  Viseme newViseme;
+  newViseme.id = nextVisemeID++;
+  
+  // Copy label (3 chars, ensure null-terminated)
+  if (label && strlen(label) >= 3) {
+    newViseme.label[0] = label[0];
+    newViseme.label[1] = label[1];
+    newViseme.label[2] = label[2];
+    newViseme.label[3] = '\0';
+  } else {
+    // Default label if not provided or too short
+    newViseme.label[0] = 'V';
+    newViseme.label[1] = 'I';
+    newViseme.label[2] = 'S';
+    newViseme.label[3] = '\0';
+  }
+  
+  newViseme.motorPositions.clear();
+  visemes.push_back(newViseme);
+  
+  Serial.print("[Viseme] Added viseme '");
+  Serial.print(newViseme.label);
+  Serial.print("' with ID ");
+  Serial.println(newViseme.id);
+  
+  return newViseme.id;
+}
+
+void VisemeBehavior::addViseme(uint8_t id, uint16_t pos40, uint16_t pos43, uint16_t pos44) {
+  // Legacy method for backwards compatibility
+  Viseme* existing = findViseme(id);
+  
+  if (existing) {
+    // Update existing viseme
+    existing->motorPositions.clear();
+    existing->motorPositions.push_back({40, pos40});
+    existing->motorPositions.push_back({43, pos43});
+    existing->motorPositions.push_back({44, pos44});
+  } else {
+    // Add new viseme
+    Viseme newViseme;
+    newViseme.id = id;
+    
+    // Default label based on ID (V + 2 digit ID)
+    newViseme.label[0] = 'V';
+    if (id < 10) {
+      newViseme.label[1] = '0' + id;
+      newViseme.label[2] = ' ';
+    } else if (id < 100) {
+      newViseme.label[1] = '0' + (id / 10);
+      newViseme.label[2] = '0' + (id % 10);
+    } else {
+      newViseme.label[1] = 'X';
+      newViseme.label[2] = 'X';
+    }
+    newViseme.label[3] = '\0';
+    
+    newViseme.motorPositions.push_back({40, pos40});
+    newViseme.motorPositions.push_back({43, pos43});
+    newViseme.motorPositions.push_back({44, pos44});
+    visemes.push_back(newViseme);
+    
+    // Update nextVisemeID if needed
+    if (id >= nextVisemeID) {
+      nextVisemeID = id + 1;
+    }
+  }
+  
+  // Update controlled motors list
+  addMotor(40);
+  addMotor(43);
+  addMotor(44);
+}
+
+// Overload to add viseme with explicit label
+void VisemeBehavior::addViseme(uint8_t id, const char* label, uint16_t pos40, uint16_t pos43, uint16_t pos44) {
+  Viseme* existing = findViseme(id);
+  
+  if (existing) {
+    // Update existing viseme
+    if (label) {
+      existing->label[0] = label[0];
+      existing->label[1] = label[1];
+      existing->label[2] = label[2];
+      existing->label[3] = '\0';
+    }
+    existing->motorPositions.clear();
+    existing->motorPositions.push_back({40, pos40});
+    existing->motorPositions.push_back({43, pos43});
+    existing->motorPositions.push_back({44, pos44});
+  } else {
+    // Add new viseme
+    Viseme newViseme;
+    newViseme.id = id;
+    
+    // Set label
+    if (label) {
+      newViseme.label[0] = label[0];
+      newViseme.label[1] = label[1];
+      newViseme.label[2] = label[2];
+      newViseme.label[3] = '\0';
+    } else {
+      // Default label
+      newViseme.label[0] = 'V';
+      if (id < 10) {
+        newViseme.label[1] = '0' + id;
+        newViseme.label[2] = ' ';
+      } else if (id < 100) {
+        newViseme.label[1] = '0' + (id / 10);
+        newViseme.label[2] = '0' + (id % 10);
+      } else {
+        newViseme.label[1] = 'X';
+        newViseme.label[2] = 'X';
+      }
+      newViseme.label[3] = '\0';
+    }
+    
+    newViseme.motorPositions.push_back({40, pos40});
+    newViseme.motorPositions.push_back({43, pos43});
+    newViseme.motorPositions.push_back({44, pos44});
+    visemes.push_back(newViseme);
+    
+    // Update nextVisemeID if needed
+    if (id >= nextVisemeID) {
+      nextVisemeID = id + 1;
+    }
+  }
+  
+  // Update controlled motors list
+  addMotor(40);
+  addMotor(43);
+  addMotor(44);
+}
+
+bool VisemeBehavior::deleteViseme(uint8_t visemeID) {
+  for (auto it = visemes.begin(); it != visemes.end(); ++it) {
+    if (it->id == visemeID) {
+      Serial.print("[Viseme] Deleted viseme ID ");
+      Serial.println(visemeID);
+      visemes.erase(it);
+      return true;
+    }
+  }
+  
+  Serial.print("[Viseme] Delete failed - unknown viseme ID ");
+  Serial.println(visemeID);
+  return false;
+}
+
+bool VisemeBehavior::setVisemeMotors(uint8_t visemeID, const std::vector<VisemeMotorPosition>& positions) {
+  Viseme* viseme = findViseme(visemeID);
+  if (!viseme) {
+    Serial.print("[Viseme] setVisemeMotors failed - unknown viseme ID ");
+    Serial.println(visemeID);
+    return false;
+  }
+  
+  // Update motor positions
+  viseme->motorPositions = positions;
+  
+  // Update controlled motors list
+  for (const auto& pos : positions) {
+    addMotor(pos.motorID);
+  }
+  
+  Serial.print("[Viseme] Updated viseme ID ");
+  Serial.print(visemeID);
+  Serial.print(" with ");
+  Serial.print(positions.size());
+  Serial.println(" motors");
+  
+  return true;
+}
+
+bool VisemeBehavior::setVisemeMotorsAndLabel(uint8_t visemeID, const char* label, const std::vector<VisemeMotorPosition>& positions) {
+  Viseme* viseme = findViseme(visemeID);
+  if (!viseme) {
+    Serial.print("[Viseme] setVisemeMotorsAndLabel failed - unknown viseme ID ");
+    Serial.println(visemeID);
+    return false;
+  }
+  
+  // Update label (3 bytes)
+  if (label) {
+    viseme->label[0] = label[0];
+    viseme->label[1] = label[1];
+    viseme->label[2] = label[2];
+    viseme->label[3] = '\0';
+  }
+  
+  // Update motor positions
+  viseme->motorPositions = positions;
+  
+  // Update controlled motors list
+  for (const auto& pos : positions) {
+    addMotor(pos.motorID);
+  }
+  
+  Serial.print("[Viseme] Updated viseme ID ");
+  Serial.print(visemeID);
+  Serial.print(" label '");
+  Serial.print(viseme->label);
+  Serial.print("' with ");
+  Serial.print(positions.size());
+  Serial.println(" motors");
+  
+  return true;
+}
+
+bool VisemeBehavior::createOrUpdateViseme(uint8_t visemeID, const char* label, const std::vector<VisemeMotorPosition>& positions) {
+  Viseme* viseme = findViseme(visemeID);
+  
+  if (viseme) {
+    // Update existing
+    return setVisemeMotorsAndLabel(visemeID, label, positions);
+  } else {
+    // Create new
+    Viseme newViseme;
+    newViseme.id = visemeID;
+    
+    // Set label
+    if (label) {
+      newViseme.label[0] = label[0];
+      newViseme.label[1] = label[1];
+      newViseme.label[2] = label[2];
+      newViseme.label[3] = '\0';
+    } else {
+      // Default label
+      newViseme.label[0] = 'V';
+      if (visemeID < 10) {
+        newViseme.label[1] = '0' + visemeID;
+        newViseme.label[2] = ' ';
+      } else if (visemeID < 100) {
+        newViseme.label[1] = '0' + (visemeID / 10);
+        newViseme.label[2] = '0' + (visemeID % 10);
+      } else {
+        newViseme.label[1] = 'X';
+        newViseme.label[2] = 'X';
+      }
+      newViseme.label[3] = '\0';
+    }
+    
+    // Set motor positions
+    newViseme.motorPositions = positions;
+    
+    visemes.push_back(newViseme);
+    
+    // Update controlled motors list
+    for (const auto& pos : positions) {
+      addMotor(pos.motorID);
+    }
+    
+    // Update nextVisemeID if needed
+    if (visemeID >= nextVisemeID) {
+      nextVisemeID = visemeID + 1;
+    }
+    
+    Serial.print("[Viseme] Created viseme ID ");
+    Serial.print(visemeID);
+    Serial.print(" label '");
+    Serial.print(newViseme.label);
+    Serial.print("' with ");
+    Serial.print(positions.size());
+    Serial.println(" motors");
+    
+    return true;
+  }
+}
+
+bool VisemeBehavior::triggerViseme(uint8_t visemeID) {
+  Viseme* viseme = findViseme(visemeID);
+  if (!viseme) {
+    Serial.print("[Viseme] Unknown viseme ID ");
+    Serial.println(visemeID);
+    return false;
+  }
+  
+  // Copy positions for this viseme
+  currentPositions = viseme->motorPositions;
+  
+  // Activate and reset timer
+  isActive = true;
+  lastTriggerTime = millis();
+  
+  Serial.print("[Viseme] Triggered '");
+  Serial.print(viseme->label);
+  Serial.print("' (ID ");
+  Serial.print(visemeID);
+  Serial.println(")");
+  
+  return true;
+}
+
+bool VisemeBehavior::update() {
+  if (!isActive) {
+    return false;
+  }
+  
+  // Check for timeout
+  unsigned long now = millis();
+  if (now - lastTriggerTime >= TIMEOUT_MS) {
+    // Timeout reached - deactivate
+    isActive = false;
+    currentPositions.clear();
+    Serial.println("[Viseme] Timeout - deactivated");
+    return false;
+  }
+  
+  return true;
+}
+
+bool VisemeBehavior::getMotorPosition(uint8_t motorID, uint16_t& position) {
+  if (!isActive) {
+    return false;
+  }
+  
+  // Look up motor in current positions
+  for (const auto& pos : currentPositions) {
+    if (pos.motorID == motorID) {
+      position = pos.position;
+      return true;
+    }
+  }
+  
+  return false;
+}
+
+// ============================================================================
+// Behavior Manager Implementation
+// ============================================================================
+
+BehaviorManager behaviorManager;
+VisemeBehavior visemeBehavior;
+
+BehaviorManager::BehaviorManager() {
+  behaviors.clear();
+  // Initialize all enabled states to false
+  for (int i = 0; i < 256; i++) {
+    enabledStates[i] = false;
+  }
+}
+
+void BehaviorManager::addBehavior(BehaviorID id, Behavior* behavior) {
+  if (behavior == nullptr) return;
+  
+  // Check if already added
+  for (const auto& entry : behaviors) {
+    if (entry.behavior == behavior || entry.id == id) return;
+  }
+  
+  behaviors.push_back({id, behavior});
+  // New behaviors are enabled by default
+  enabledStates[id] = true;
+}
+
+void BehaviorManager::removeBehavior(Behavior* behavior) {
+  behaviors.erase(
+    std::remove_if(behaviors.begin(), behaviors.end(),
+                   [behavior](const BehaviorEntry& entry) {
+                     return entry.behavior == behavior;
+                   }),
+    behaviors.end()
+  );
+}
+
+void BehaviorManager::setBehaviorEnabled(BehaviorID id, bool enabled) {
+  enabledStates[id] = enabled;
+}
+
+bool BehaviorManager::isBehaviorEnabled(BehaviorID id) const {
+  return enabledStates[id];
+}
+
+uint8_t BehaviorManager::getBehaviorCount() const {
+  return behaviors.size();
+}
+
+bool BehaviorManager::getBehaviorInfo(uint8_t index, BehaviorID& id, bool& enabled) const {
+  if (index >= behaviors.size()) {
+    return false;
+  }
+  
+  id = behaviors[index].id;
+  enabled = enabledStates[id];
+  return true;
+}
+
+void BehaviorManager::update() {
+  // Update all enabled behaviors
+  for (const auto& entry : behaviors) {
+    if (entry.behavior && enabledStates[entry.id]) {
+      entry.behavior->update();
+    }
+  }
+}
+
+bool BehaviorManager::getMotorPosition(uint8_t motorID, uint16_t& position) {
+  // Check all enabled behaviors to see if any wants to control this motor
+  for (const auto& entry : behaviors) {
+    if (entry.behavior && enabledStates[entry.id] && 
+        entry.behavior->getMotorPosition(motorID, position)) {
+      return true;  // Found an enabled behavior controlling this motor
+    }
+  }
+  return false;  // No enabled behavior controlling this motor
+}

behaviors.h

@@ -0,0 +1,265 @@
+#pragma once
+#include <Arduino.h>
+#include <vector>
+#include "sensors.h"
+#include "robotconfig.h"
+#include "noise.h"
+
+// ============================================================================
+// Behavior IDs
+// ============================================================================
+
+enum BehaviorID : uint8_t {
+  BEHAVIOR_FOCUS = 1,  // Focus behavior (radar tracking)
+  BEHAVIOR_IDLE = 2,   // Idle behavior (perlin noise for all motors)
+  BEHAVIOR_VISEME = 3, // Viseme behavior (mouth motor positions)
+};
+
+// ============================================================================
+// Base Behavior Class
+// ============================================================================
+
+class Behavior {
+public:
+  Behavior();
+  virtual ~Behavior() = default;
+
+  // Get list of motor IDs this behavior controls
+  const std::vector<uint8_t>& getControlledMotors() const { return controlledMotors; }
+
+  // Add a motor to the controlled list
+  void addMotor(uint8_t motorID);
+
+  // Remove a motor from the controlled list
+  void removeMotor(uint8_t motorID);
+
+  // Clear all controlled motors
+  void clearMotors();
+
+  // Virtual method to update the behavior (called each frame)
+  // Returns true if the behavior is active and wants to control motors
+  virtual bool update() = 0;
+
+  // Virtual method to get the desired position for a motor
+  // Returns true if this behavior wants to control this motor, false otherwise
+  virtual bool getMotorPosition(uint8_t motorID, uint16_t& position) = 0;
+
+protected:
+  std::vector<uint8_t> controlledMotors;
+};
+
+// ============================================================================
+// Focus Behavior - Tracks radar targets with eyes/neck
+// ============================================================================
+
+class FocusBehavior : public Behavior {
+public:
+  FocusBehavior();
+
+  // Update behavior - check radar for targets
+  bool update() override;
+
+  // Get motor position for a controlled motor
+  bool getMotorPosition(uint8_t motorID, uint16_t& position) override;
+
+private:
+  bool isActive;
+  uint16_t eyePosition;      // Current interpolated position for motors 14 and 15
+  uint16_t neckPosition;     // Current interpolated position for motor 27
+  
+  // Target positions
+  uint16_t targetEyePosition;
+  uint16_t targetNeckPosition;
+  
+  // Timing
+  unsigned long targetDetectedTime;  // When target was first detected
+  unsigned long neckStartTime;       // When neck rotation should start
+  bool neckRotating;                 // Whether neck is actively rotating
+  
+  // Configuration
+  static constexpr uint8_t FOCUS_MOTOR_1 = 14;  // Eye motor 1
+  static constexpr uint8_t FOCUS_MOTOR_2 = 15;  // Eye motor 2
+  static constexpr uint8_t NECK_MOTOR = 27;     // Neck motor
+  
+  // Eye motor position range (motors 14 and 15)
+  static constexpr uint16_t EYE_POSITION_CENTER = 2200;
+  static constexpr uint16_t EYE_POSITION_MIN = 1700;
+  static constexpr uint16_t EYE_POSITION_MAX = 2500;
+  
+  // Neck motor position range (motor 27)
+  static constexpr uint16_t NECK_POSITION_CENTER = 2000;
+  static constexpr uint16_t NECK_POSITION_MIN = 1000;
+  static constexpr uint16_t NECK_POSITION_MAX = 3000;
+  
+  static constexpr unsigned long NECK_DELAY_MS = 1000;  // 1 second delay before neck moves
+  static constexpr float INTERPOLATION_SPEED = 0.05f;   // Smooth interpolation factor for eyes (0-1, higher = faster)
+  static constexpr float NECK_INTERPOLATION_SPEED = 0.03f;  // Slower interpolation for neck (smoother motion)
+  static constexpr float EYE_CENTERING_SPEED = 0.03f;   // Speed at which eyes center when neck rotates
+
+  // Calculate motor position from radar angle (in degrees)
+  uint16_t calculateEyePositionFromRadarAngle(float radarAngle);
+  uint16_t calculateNeckPositionFromRadarAngle(float radarAngle);
+  
+  // Smooth interpolation helper (linear interpolation)
+  uint16_t lerp(uint16_t current, uint16_t target, float t);
+};
+
+// ============================================================================
+// Idle Behavior - Adds perlin noise to all motors for natural idle motion
+// ============================================================================
+
+class IdleBehavior : public Behavior {
+public:
+  IdleBehavior();
+
+  // Initialize with list of motor IDs to control
+  void initMotors(const std::vector<uint8_t>& motorIDs);
+
+  // Update behavior - calculates new noise positions
+  bool update() override;
+
+  // Get motor position for a controlled motor
+  bool getMotorPosition(uint8_t motorID, uint16_t& position) override;
+
+private:
+  // Store current positions for each motor (indexed by motor ID)
+  uint16_t motorPositions[256];
+  
+  // Time offset for perlin noise animation
+  unsigned long startTime;
+  
+  // Configuration
+  static constexpr uint16_t POSITION_CENTER = 2047;
+  static constexpr uint16_t NOISE_RANGE = 100;       // ±500 from center
+  static constexpr float NOISE_SPEED = 0.000125f;    // How fast noise evolves (slower = smoother, 4x slower)
+  static constexpr uint16_t MOTOR_SEED_OFFSET = 100; // Seed offset between motors for variety
+};
+
+// ============================================================================
+// Viseme Behavior - Controls mouth motors for speech
+// ============================================================================
+
+// Motor position within a viseme
+struct VisemeMotorPosition {
+  uint8_t motorID;
+  uint16_t position;
+};
+
+// Viseme definition: ID, label (3 chars), and motor positions
+struct Viseme {
+  uint8_t id;
+  char label[4];  // 3 characters + null terminator
+  std::vector<VisemeMotorPosition> motorPositions;
+};
+
+class VisemeBehavior : public Behavior {
+public:
+  VisemeBehavior();
+
+  // Add a viseme with a 3-char label (auto-assigns ID)
+  // Returns the assigned viseme ID
+  uint8_t addViseme(const char* label);
+  
+  // Legacy: Add a viseme with specific ID and motor positions (for backwards compatibility)
+  void addViseme(uint8_t id, uint16_t pos40, uint16_t pos43, uint16_t pos44);
+  
+  // Add a viseme with specific ID, label, and motor positions
+  void addViseme(uint8_t id, const char* label, uint16_t pos40, uint16_t pos43, uint16_t pos44);
+  
+  // Delete a viseme by ID
+  // Returns true if deleted, false if not found
+  bool deleteViseme(uint8_t visemeID);
+  
+  // Set motor positions for a viseme
+  // Returns true if viseme found and updated, false otherwise
+  bool setVisemeMotors(uint8_t visemeID, const std::vector<VisemeMotorPosition>& positions);
+  
+  // Set motor positions and label for a viseme
+  // Returns true if viseme found and updated, false otherwise
+  bool setVisemeMotorsAndLabel(uint8_t visemeID, const char* label, const std::vector<VisemeMotorPosition>& positions);
+  
+  // Create or update a viseme with specific ID, label, and motor positions
+  // Returns true on success
+  bool createOrUpdateViseme(uint8_t visemeID, const char* label, const std::vector<VisemeMotorPosition>& positions);
+  
+  // Get all visemes (for VLST command)
+  const std::vector<Viseme>& getVisemes() const { return visemes; }
+  
+  // Trigger a viseme by ID - activates the behavior and sets positions
+  // Returns true if viseme was found, false otherwise
+  bool triggerViseme(uint8_t visemeID);
+
+  // Update behavior - checks for timeout and deactivates
+  bool update() override;
+
+  // Get motor position for a controlled motor
+  bool getMotorPosition(uint8_t motorID, uint16_t& position) override;
+
+private:
+  bool isActive;
+  unsigned long lastTriggerTime;
+  uint8_t nextVisemeID;  // Auto-increment ID for new visemes
+  
+  // Current active motor positions (when triggered)
+  std::vector<VisemeMotorPosition> currentPositions;
+  
+  // Registered visemes
+  std::vector<Viseme> visemes;
+  
+  // Configuration
+  static constexpr unsigned long TIMEOUT_MS = 3000;  // 3 second timeout
+  static constexpr uint16_t DEFAULT_POSITION = 2047; // Center/rest position
+  
+  // Helper to find viseme by ID
+  Viseme* findViseme(uint8_t id);
+};
+
+// ============================================================================
+// Behavior Manager - Manages active behaviors and resolves motor conflicts
+// ============================================================================
+
+class BehaviorManager {
+public:
+  BehaviorManager();
+  
+  // Add a behavior to the manager with an ID
+  void addBehavior(BehaviorID id, Behavior* behavior);
+  
+  // Remove a behavior from the manager
+  void removeBehavior(Behavior* behavior);
+  
+  // Enable/disable a behavior by ID
+  void setBehaviorEnabled(BehaviorID id, bool enabled);
+  
+  // Check if a behavior is enabled
+  bool isBehaviorEnabled(BehaviorID id) const;
+  
+  // Get count of registered behaviors
+  uint8_t getBehaviorCount() const;
+  
+  // Get behavior info at index (for iteration)
+  // Returns true if index is valid and fills out id and enabled
+  bool getBehaviorInfo(uint8_t index, BehaviorID& id, bool& enabled) const;
+  
+  // Update all enabled behaviors (call each frame)
+  void update();
+  
+  // Check if a behavior wants to control a specific motor
+  // Returns true if a behavior provides a position, false otherwise
+  bool getMotorPosition(uint8_t motorID, uint16_t& position);
+
+private:
+  struct BehaviorEntry {
+    BehaviorID id;
+    Behavior* behavior;
+  };
+  
+  std::vector<BehaviorEntry> behaviors;
+  bool enabledStates[256] = {false};  // Track enabled state by ID
+};
+
+// Global behavior manager instance
+extern BehaviorManager behaviorManager;
+
+// Global viseme behavior instance (for command access)
+extern VisemeBehavior visemeBehavior;

commands.cpp

@@ -0,0 +1,958 @@
+#include "commands.h"
+#include "nodegraph.h"
+#include "sensors.h"
+#include "behaviors.h"
+#include "esp_system.h"
+#include "soc/rtc_cntl_reg.h"
+#include <vector>
+#include <unordered_map>
+
+// External references
+extern RobotConfig config;
+extern ServoManager servoManager;
+
+// Global state instances
+AnimationState animState;
+MotorStreamState motorStream;
+
+// ============================================================================
+// Chunked File Save Session (FSAV)
+// ============================================================================
+struct SaveSession {
+  bool active = false;
+  uint16_t totalChunks = 0;
+  uint16_t receivedChunks = 0;
+  uint32_t totalSize = 0;
+  uint16_t chunkSize = 0;  // size of non-final chunks
+  std::vector<uint8_t> buffer;
+  std::vector<bool> received;
+} g_save;
+
+// Helper: reset save session
+static void resetSaveSession() {
+  g_save.active = false;
+  g_save.totalChunks = 0;
+  g_save.receivedChunks = 0;
+  g_save.totalSize = 0;
+  g_save.chunkSize = 0;
+  g_save.buffer.clear();
+  g_save.received.clear();
+}
+
+// ============================================================================
+// AnimationState
+// ============================================================================
+
+void AnimationState::play(PlayMode mode, uint8_t repeats, uint16_t startFrame) {
+  current = &animation;
+  current->setActive(true);
+  playMode = mode;
+  repeatsRemaining = repeats;
+  this->startFrame = startFrame;
+  playGeneration++;  // Signal that a new animation has started
+}
+
+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);
+  }
+  else if (tagMatches(tag, Tag::FSTP)) {
+    handleFileStop(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);
+  }
+  // Behaviors
+  else if (tagMatches(tag, Tag::BHVR)) {
+    handleBehavior(payload, len);
+  }
+  else if (tagMatches(tag, Tag::BLST)) {
+    handleBehaviorList(payload, len);
+  }
+  // Visemes
+  else if (tagMatches(tag, Tag::VLST)) {
+    handleVisemeList(payload, len);
+  }
+  else if (tagMatches(tag, Tag::VADD)) {
+    handleVisemeAdd(payload, len);
+  }
+  else if (tagMatches(tag, Tag::VDEL)) {
+    handleVisemeDelete(payload, len);
+  }
+  else if (tagMatches(tag, Tag::VSET)) {
+    handleVisemeSet(payload, len);
+  }
+  else if (tagMatches(tag, Tag::VSME)) {
+    handleVisemeTrigger(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();
+  const uint16_t CHUNK_SIZE = 1024;  // match sender chunking
+  uint16_t totalChunks = (fileSize + CHUNK_SIZE - 1) / CHUNK_SIZE;
+
+  // Prepare static header fields
+  uint8_t header[8];
+  header[0] = totalChunks & 0xFF;
+  header[1] = (totalChunks >> 8) & 0xFF;
+  header[4] = fileSize & 0xFF;
+  header[5] = (fileSize >> 8) & 0xFF;
+  header[6] = (fileSize >> 16) & 0xFF;
+  header[7] = (fileSize >> 24) & 0xFF;
+
+  for (uint16_t chunkIndex = 0; chunkIndex < totalChunks; chunkIndex++) {
+    size_t offset = (size_t)chunkIndex * CHUNK_SIZE;
+    uint16_t thisChunk = (uint16_t)min((size_t)CHUNK_SIZE, fileSize - offset);
+
+    // Fill per-chunk fields
+    header[2] = chunkIndex & 0xFF;
+    header[3] = (chunkIndex >> 8) & 0xFF;
+
+    // Build payload: header + chunkData
+    std::vector<uint8_t> payloadBuf;
+    payloadBuf.reserve(8 + thisChunk);
+    payloadBuf.insert(payloadBuf.end(), header, header + 8);
+
+    // Read chunk data directly into payload buffer
+    size_t startIdx = payloadBuf.size();
+    payloadBuf.resize(startIdx + thisChunk);
+    file.seek(offset);
+    file.read(payloadBuf.data() + startIdx, thisChunk);
+
+    sendPacket(Tag::FLOAD, payloadBuf.data(), payloadBuf.size());
+    delay(2);  // small pacing to avoid overwhelming host
+  }
+
+  file.close();
+}
+
+void handleFileSave(const uint8_t* payload, uint16_t len) {
+  // Chunked protocol:
+  // [totalChunks:2][chunkIndex:2][totalSize:4][chunkData:...]
+  if (len < 8) {
+    sendNack(Tag::FSAVE, "Payload too short");
+    return;
+  }
+
+  uint16_t totalChunks = payload[0] | (payload[1] << 8);
+  uint16_t chunkIndex = payload[2] | (payload[3] << 8);
+  uint32_t totalSize = payload[4] | (payload[5] << 8) | (payload[6] << 16) | (payload[7] << 24);
+  const uint8_t* chunkData = payload + 8;
+  uint16_t chunkLen = len - 8;
+
+  // Basic validation
+  if (totalChunks == 0) {
+    sendNack(Tag::FSAVE, "totalChunks=0");
+    return;
+  }
+  if (chunkIndex >= totalChunks) {
+    sendNack(Tag::FSAVE, "chunkIndex out of range");
+    return;
+  }
+  if (totalSize == 0) {
+    sendNack(Tag::FSAVE, "totalSize=0");
+    return;
+  }
+
+  // Start new session if needed
+  if (!g_save.active || g_save.totalSize != totalSize || g_save.totalChunks != totalChunks) {
+    g_save.active = true;
+    g_save.totalChunks = totalChunks;
+    g_save.receivedChunks = 0;
+    g_save.totalSize = totalSize;
+    g_save.chunkSize = chunkLen;  // assume first chunk size is the standard chunk size
+    g_save.buffer.assign(totalSize, 0);
+    g_save.received.assign(totalChunks, false);
+  }
+
+  // Calculate offset
+  uint32_t offset = (uint32_t)chunkIndex * (uint32_t)g_save.chunkSize;
+  // For safety, allow last chunk to be smaller
+  if (offset + chunkLen > g_save.totalSize) {
+    sendNack(Tag::FSAVE, "chunk overflow");
+    return;
+  }
+
+  // If not already received, copy in
+  if (!g_save.received[chunkIndex]) {
+    memcpy(g_save.buffer.data() + offset, chunkData, chunkLen);
+    g_save.received[chunkIndex] = true;
+    g_save.receivedChunks++;
+  }
+
+  // If not all chunks yet, ACK chunk and return
+  if (g_save.receivedChunks < g_save.totalChunks) {
+    sendAck(Tag::FSAVE);  // per-chunk ACK
+    return;
+  }
+
+  // All chunks received - parse and save animation
+  bool ok = parseAndSaveAnimation(g_save.buffer.data(), g_save.buffer.size(), animState.animation);
+  g_save.active = false;
+  g_save.buffer.clear();
+  g_save.received.clear();
+
+  if (ok) {
+    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 < 6) {  // Minimum: filenameLen(2) + filename(1) + mode(1) + repeats(1) + startFrame(2)
+    sendNack(Tag::FPLAY, "Invalid request");
+    return;
+  }
+
+  uint16_t filenameLen = payload[0] | (payload[1] << 8);
+  if (len < 2 + filenameLen + 4) {  // filenameLen + filename + mode + repeats + startFrame
+    sendNack(Tag::FPLAY, "Invalid payload length");
+    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];
+  uint16_t startFrame = payload[offset + 2] | (payload[offset + 3] << 8);
+
+  // Debug: show parsed startFrame
+  sendMessage("FPLAY parsed - startFrame bytes: [" + String(payload[offset + 2]) + ", " + String(payload[offset + 3]) + "] = " + String(startFrame));
+
+  animState.animation.clear();
+  String fullPath = "/" + String(filename);
+
+  if (animState.animation.loadFromFile(fullPath.c_str())) {
+    animState.play(mode, repeats, startFrame);
+    sendAck(Tag::FPLAY);
+    sendMessage("Playing: " + String(filename) + " from frame " + String(startFrame));
+    sendMessage("animState.startFrame stored as: " + String(animState.startFrame));
+  } else {
+    sendNack(Tag::FPLAY, "Load failed");
+  }
+}
+
+void handleFileStop(const uint8_t* payload, uint16_t len) {
+  // FSTP has no payload (len should be 0, but we don't strictly require it)
+  animState.stop();
+  sendAck(Tag::FSTP);
+  sendMessage("Animation stopped");
+}
+
+// ============================================================================
+// Behavior Handler
+// ============================================================================
+
+void handleBehavior(const uint8_t* payload, uint16_t len) {
+  // BHVR payload: [behaviorID (1 byte)][enable (1 byte)]
+  if (len < 2) {
+    sendNack(Tag::BHVR, "Invalid payload length");
+    return;
+  }
+  
+  uint8_t behaviorID = payload[0];
+  uint8_t enable = payload[1];
+  
+  // Validate behavior ID
+  if (behaviorID == 0 || behaviorID > 255) {
+    sendNack(Tag::BHVR, "Invalid behavior ID");
+    return;
+  }
+  
+  // Enable/disable the behavior
+  bool enabled = (enable != 0);
+  behaviorManager.setBehaviorEnabled(static_cast<BehaviorID>(behaviorID), enabled);
+  
+  // Save config to persist the behavior state change
+  config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior);
+  
+  // Send acknowledgment
+  sendAck(Tag::BHVR);
+  
+  // Send status message
+  String status = enabled ? "enabled" : "disabled";
+  sendMessage("Behavior " + String(behaviorID) + " " + status);
+}
+
+void handleBehaviorList(const uint8_t* payload, uint16_t len) {
+  // BLST payload: none (can be empty)
+  // Response: [count (1 byte)][behaviorID1 (1 byte)][enabled1 (1 byte)]...
+  
+  uint8_t count = behaviorManager.getBehaviorCount();
+  
+  // Build response payload: [count][id1][enabled1][id2][enabled2]...
+  std::vector<uint8_t> response;
+  response.push_back(count);
+  
+  for (uint8_t i = 0; i < count; i++) {
+    BehaviorID id;
+    bool enabled;
+    if (behaviorManager.getBehaviorInfo(i, id, enabled)) {
+      response.push_back(static_cast<uint8_t>(id));
+      response.push_back(enabled ? 1 : 0);
+    }
+  }
+  
+  // Send response packet
+  sendPacket(Tag::BLST, response.data(), response.size());
+  
+  // Also send debug message with behavior names
+  String msg = "Behaviors: ";
+  for (uint8_t i = 0; i < count; i++) {
+    BehaviorID id;
+    bool enabled;
+    if (behaviorManager.getBehaviorInfo(i, id, enabled)) {
+      if (i > 0) msg += ", ";
+      String name;
+      switch (id) {
+        case BEHAVIOR_FOCUS:
+          name = "Focus";
+          break;
+        case BEHAVIOR_IDLE:
+          name = "Idle";
+          break;
+        case BEHAVIOR_VISEME:
+          name = "Viseme";
+          break;
+        default:
+          name = "Unknown(" + String(static_cast<uint8_t>(id)) + ")";
+          break;
+      }
+      msg += name + "(" + String(static_cast<uint8_t>(id)) + ")=";
+      msg += enabled ? "ON" : "OFF";
+    }
+  }
+  sendMessage(msg);
+}
+
+// ============================================================================
+// Viseme Handlers
+// ============================================================================
+
+void handleVisemeList(const uint8_t* payload, uint16_t len) {
+  // VLST - returns all visemes with their labels and motor positions
+  const std::vector<Viseme>& visemes = visemeBehavior.getVisemes();
+  
+  // Build response payload
+  std::vector<uint8_t> response;
+  response.push_back(visemes.size());  // count
+  
+  for (const Viseme& v : visemes) {
+    response.push_back(v.id);  // viseme_id
+    
+    // Label (3 bytes)
+    response.push_back(v.label[0]);
+    response.push_back(v.label[1]);
+    response.push_back(v.label[2]);
+    
+    // Motor positions
+    response.push_back(v.motorPositions.size());  // motor_count
+    for (const VisemeMotorPosition& mp : v.motorPositions) {
+      response.push_back(mp.motorID);
+      response.push_back(mp.position & 0xFF);         // position_low
+      response.push_back((mp.position >> 8) & 0xFF);  // position_high
+    }
+  }
+  
+  sendPacket(Tag::VLST, response.data(), response.size());
+}
+
+void handleVisemeAdd(const uint8_t* payload, uint16_t len) {
+  // VADD payload: [label: 3 bytes]
+  if (len < 3) {
+    sendNack(Tag::VADD, "Invalid payload length (need 3-byte label)");
+    return;
+  }
+  
+  // Extract label (3 bytes)
+  char label[4];
+  label[0] = payload[0];
+  label[1] = payload[1];
+  label[2] = payload[2];
+  label[3] = '\0';
+  
+  // Add the viseme
+  uint8_t newID = visemeBehavior.addViseme(label);
+  
+  // Save config to persist the new viseme
+  config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior);
+  
+  // Send ACK with the new ID
+  uint8_t ackPayload[1] = { newID };
+  sendPacket(Tag::ACK, ackPayload, 1);
+}
+
+void handleVisemeDelete(const uint8_t* payload, uint16_t len) {
+  // VDEL payload: [viseme_id: 1 byte]
+  if (len < 1) {
+    sendNack(Tag::VDEL, "Invalid payload length");
+    return;
+  }
+  
+  uint8_t visemeID = payload[0];
+  
+  if (visemeBehavior.deleteViseme(visemeID)) {
+    // Save config to persist the deletion
+    config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior);
+    sendAck(Tag::VDEL);
+  } else {
+    sendNack(Tag::VDEL, "Viseme not found");
+  }
+}
+
+void handleVisemeSet(const uint8_t* payload, uint16_t len) {
+  // VSET payload: [viseme_id: 1][label: 3 bytes][motor_count: 1][motor_id: 1][pos_low: 1][pos_high: 1]...
+  if (len < 5) {  // Minimum: viseme_id(1) + label(3) + motor_count(1)
+    sendNack(Tag::VSET, "Invalid payload length");
+    return;
+  }
+  
+  uint8_t visemeID = payload[0];
+  
+  // Extract label (3 bytes)
+  char label[4];
+  label[0] = payload[1];
+  label[1] = payload[2];
+  label[2] = payload[3];
+  label[3] = '\0';
+  
+  uint8_t motorCount = payload[4];
+  
+  // Calculate expected length: viseme_id(1) + label(3) + motor_count(1) + motors(motorCount * 3)
+  if (len < 5 + motorCount * 3) {
+    sendNack(Tag::VSET, "Motor data truncated");
+    return;
+  }
+  
+  // Parse motor positions
+  std::vector<VisemeMotorPosition> positions;
+  for (uint8_t i = 0; i < motorCount; i++) {
+    uint16_t offset = 5 + i * 3;  // Start after viseme_id(1) + label(3) + motor_count(1)
+    VisemeMotorPosition mp;
+    mp.motorID = payload[offset];
+    mp.position = payload[offset + 1] | (payload[offset + 2] << 8);  // Little-endian
+    positions.push_back(mp);
+  }
+  
+  // Use createOrUpdateViseme so VSET can create new visemes or update existing ones
+  if (visemeBehavior.createOrUpdateViseme(visemeID, label, positions)) {
+    // Save config to persist the changes
+    config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior);
+    sendAck(Tag::VSET);
+  } else {
+    sendNack(Tag::VSET, "Failed to update viseme");
+  }
+}
+
+void handleVisemeTrigger(const uint8_t* payload, uint16_t len) {
+  // VSME payload: [viseme_id: 1 byte]
+  // Fire-and-forget - no response expected
+  if (len < 1) {
+    return;  // Silent fail for fire-and-forget
+  }
+  
+  uint8_t visemeID = payload[0];
+  visemeBehavior.triggerViseme(visemeID);
+  // No response sent - fire and forget
+}
+
+// ============================================================================
+// 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) {
+  const uint8_t* ptr = payload;
+  uint16_t remaining = len;
+
+  // Optional filename block (only present if sender included it)
+  String filename = "received.anim";
+  if (remaining >= 4 && strncmp((const char*)ptr, "ANIM", 4) != 0) {
+    if (remaining < 2) return false;
+    uint16_t filenameLen = ptr[0] | (ptr[1] << 8);
+    ptr += 2;
+    remaining -= 2;
+    if (filenameLen > 127 || remaining < filenameLen) return false;
+    char fname[128];
+    memcpy(fname, ptr, filenameLen);
+    fname[filenameLen] = '\0';
+    filename = fname;
+    ptr += filenameLen;
+    remaining -= filenameLen;
+  }
+
+  // Header: 16 bytes
+  if (remaining < 16) 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);
+  ptr += 16;
+  remaining -= 16;
+
+  if (animation.header.version == 2) {
+    // Version 2: Frame data block
+    // Calculate motor count from remaining data
+    if (animation.header.frameCount == 0) return false;
+    uint16_t frameDataSize = remaining;
+    uint16_t frameSize = frameDataSize / animation.header.frameCount;
+    uint16_t motorCount = frameSize / sizeof(MotorPosition);
+    
+    if (frameSize % sizeof(MotorPosition) != 0) return false;
+    if (frameDataSize < animation.header.frameCount * motorCount * sizeof(MotorPosition)) return false;
+    
+    animation.clearFrameData();
+    
+    // Read all frames
+    for (uint16_t frameIndex = 0; frameIndex < animation.header.frameCount; frameIndex++) {
+      std::vector<MotorPosition> frame;
+      frame.reserve(motorCount);
+      
+      for (uint16_t motorIndex = 0; motorIndex < motorCount; motorIndex++) {
+        if (remaining < sizeof(MotorPosition)) return false;
+        MotorPosition motorPos;
+        memcpy(&motorPos, ptr, sizeof(MotorPosition));
+        frame.push_back(motorPos);
+        ptr += sizeof(MotorPosition);
+        remaining -= sizeof(MotorPosition);
+      }
+      
+      animation.setFrameData(frameIndex, frame);
+    }
+  } else {
+    // Version 1: Curve count (at start of curve block)
+    if (remaining < 2) return false;
+    uint16_t curveCount = ptr[0] | (ptr[1] << 8);
+    ptr += 2;
+    remaining -= 2;
+
+    // Curves (17 bytes each, packed)
+    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;
+
+    // Node graph (whatever remains)
+    if (remaining > 0) {
+      loadNodeGraph(ptr, remaining, animation.nodeGraph);
+      animation.nodeGraph.bindAnimationContext(&animation);
+    }
+  }
+
+  // Save to file
+  String fullPath = "/" + filename;
+  animation.saveToFile(fullPath.c_str());
+  sendMessage("Saved: " + 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,97 @@
+#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;
+  uint16_t startFrame = 0;  // Frame to start playback from
+  uint8_t playGeneration = 0;  // Increments each time play() is called
+
+  void play(PlayMode mode, uint8_t repeats = 0, uint16_t startFrame = 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);
+void handleFileStop(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);
+
+// Behaviors
+void handleBehavior(const uint8_t* payload, uint16_t len);
+void handleBehaviorList(const uint8_t* payload, uint16_t len);
+
+// Visemes
+void handleVisemeList(const uint8_t* payload, uint16_t len);
+void handleVisemeAdd(const uint8_t* payload, uint16_t len);
+void handleVisemeDelete(const uint8_t* payload, uint16_t len);
+void handleVisemeSet(const uint8_t* payload, uint16_t len);
+void handleVisemeTrigger(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);

feetech.cpp

@@ -377,11 +377,16 @@ uint8_t Feetech::getMoving(uint8_t id) {
   return waitOnData1Byte(10);
 }
 
-// Multiplier could be wrong
+// STS/SMS: actual current at 0x45, SCS: use load at 0x3C as proxy
 uint16_t Feetech::getCurrent(uint8_t id) {
-  sendRequest(id, REQUEST_CURRENT_CURRENT, 2);
+  if (feetechMode == MODE_STS || feetechMode == MODE_SMSA || feetechMode == MODE_SMSB) {
-  //return waitOnData2Bytes(10) * 0.01; FLOAT
+    sendRequest(id, REQUEST_CURRENT_CURRENT, 2);
-  return waitOnData2Bytes(10);
+    return waitOnData2Bytes(10);
+  } else {
+    // SCS doesn't have current register, use load as proxy
+    sendRequest(id, REQUEST_CURRENT_LOAD, 2);
+    return flipBytes(waitOnData2Bytes(10));
+  }
 }
 
 void Feetech::sendRequest(uint8_t id, byte instruction, uint8_t byteCount) {

ls_firmware.ino

@@ -0,0 +1,738 @@
+/**
+ * HansonServo Firmware
+ *
+ * Unified robot controller with:
+ * - Feetech servo control (SCS/STS)
+ * - Animation playback via node graphs
+ * - ADXL345 IMU
+ * - RD-03D mmWave radar
+ * - CRC16 tagged packet protocol
+ *
+ * Protocol: 0xA5 0x5A [TAG 4B][LEN 2B][SEQ 2B][PAYLOAD][CRC16 2B]
+ */
+
+// Configuration defines
+#define ENABLE_SERIAL_PASSTHROUGH                                              \
+  false // Set true to use Feetech app comms straight to servos
+
+#include "robotconfig.h"
+#include "animation.h"
+#include "commands.h"
+#include "motorcontrol.h"
+#include "motoraid.h"
+#include "nodegraph.h"
+#include "protocol.h"
+#include "sensors.h"
+#include "behaviors.h"
+#include <FFat.h>
+
+
+// ============================================================================
+// Global State
+// ============================================================================
+
+RobotConfig config;
+
+// Timing constants
+constexpr uint16_t FRAME_RATE = 48;
+constexpr uint16_t FRAME_INTERVAL_MS = 1000 / FRAME_RATE;
+constexpr uint16_t MOTOR_UPDATE_INTERVAL_MS = 50;
+constexpr uint32_t HEARTBEAT_INTERVAL_MS = 1000;
+
+// ============================================================================
+// Utility Functions
+// ============================================================================
+
+uint16_t getSineWaveValue(unsigned long centiseconds) {
+  constexpr uint16_t WAVE_PERIOD_CS = 400; // 4 seconds
+  constexpr uint16_t WAVE_MAX = 4095;
+
+  float theta = (2.0 * PI * centiseconds) / WAVE_PERIOD_CS;
+  float sine = sin(theta);
+  float scaled = (sine + 1.0) * (WAVE_MAX / 2.0);
+  return (uint16_t)round(scaled);
+}
+
+// ============================================================================
+// Unified Motor Command Application
+// ============================================================================
+
+// Apply motor commands from behaviors and/or animations
+// This function works whether an animation is active or not
+// Behaviors have priority - they override animation positions
+void applyMotorCommands(const std::vector<uint8_t>& motorIDs, 
+                        const std::vector<uint16_t>& positions) {
+  if (motorIDs.empty()) return;
+  
+  std::vector<uint8_t> finalMotorIDs;
+  std::vector<uint16_t> finalPositions;
+  std::vector<uint16_t> speeds;
+  
+  for (size_t i = 0; i < motorIDs.size(); i++) {
+    uint8_t motorID = motorIDs[i];
+    uint16_t finalPosition = positions[i];
+    
+    // Check if a behavior wants to control this motor (behaviors have priority)
+    uint16_t behaviorPosition;
+    if (behaviorManager.getMotorPosition(motorID, behaviorPosition)) {
+      // Behavior is controlling this motor, use its position instead
+      finalPosition = behaviorPosition;
+    }
+    
+    finalMotorIDs.push_back(motorID);
+    finalPositions.push_back(finalPosition);
+    speeds.push_back(0);
+    config.setMotorPosition(motorID, finalPosition);
+    config.setMotorEnabled(motorID, true);
+  }
+  
+  // Send all positions in one sync write
+  if (!finalMotorIDs.empty()) {
+    servoManager.syncWritePositions(finalMotorIDs.data(), finalPositions.data(),
+                                    speeds.data(), finalMotorIDs.size(), config, 0);
+  }
+}
+
+// Apply motor commands from behaviors only (when no animation is active)
+void applyBehaviorMotorCommands() {
+  // Get all motors that behaviors want to control
+  std::vector<uint8_t> motorIDs;
+  std::vector<uint16_t> positions;
+  
+  // Check all configured motors to see if any behavior wants to control them
+  for (const Motor& motor : config.motors) {
+    uint16_t position;
+    if (behaviorManager.getMotorPosition(motor.motorID, position)) {
+      motorIDs.push_back(motor.motorID);
+      positions.push_back(position);
+    }
+  }
+  
+  // Apply the behavior-controlled motors
+  if (!motorIDs.empty()) {
+    applyMotorCommands(motorIDs, positions);
+  }
+}
+
+// ============================================================================
+// Protocol Handler
+// ============================================================================
+
+void handleProtocol() {
+  if (receivePacket()) {
+    dispatchCommand();
+  }
+}
+
+// ============================================================================
+// Serial Passthrough (USB ↔ Servo Serial)
+// ============================================================================
+
+#if ENABLE_SERIAL_PASSTHROUGH
+void handleSerialPassthrough() {
+  // Forward USB Serial → Servo Serial1
+  while (Serial.available()) {
+    Serial1.write(Serial.read());
+  }
+
+  // Forward Servo Serial1 → USB Serial
+  while (Serial1.available()) {
+    Serial.write(Serial1.read());
+  }
+}
+#endif
+
+// ============================================================================
+// Animation Playback
+// ============================================================================
+
+// Dispatcher: calls the appropriate animation function based on version
+void runAnimation() {
+  if (!animState.current || !animState.current->isActive()) {
+    return;
+  }
+
+  if (animState.current->header.version == 2) {
+    runFrameAnimation();
+  } else {
+    runNodeAnimation();
+  }
+}
+
+// Version 2: Frame-by-frame animation playback
+void runFrameAnimation() {
+  static uint32_t lastTickTime = 0;
+  static uint32_t currentTick = 0;
+  static uint8_t lastGeneration = 0;
+
+  if (!animState.current || !animState.current->isActive()) {
+    return;
+  }
+
+  // Reset tick when a new animation starts (detected by generation change)
+  if (lastGeneration != animState.playGeneration) {
+    currentTick = animState.startFrame;
+    lastTickTime = millis();
+    lastGeneration = animState.playGeneration;
+    sendMessage("V2 Animation started, generation: " + String(lastGeneration) + ", startFrame: " + String(animState.startFrame));
+  }
+
+  config.enableAllMotors();
+
+  // Calculate frame interval from animation's frame rate
+  uint16_t frameIntervalMs = 1000 / animState.current->header.frameRate;
+  if (frameIntervalMs == 0) frameIntervalMs = 1;  // Safety: prevent division by zero
+
+  uint32_t now = millis();
+  if (now - lastTickTime < frameIntervalMs)
+    return;
+  lastTickTime = now;
+
+  // Get frame data for current tick
+  const std::vector<MotorPosition>* frameData = animState.current->getFrameData(currentTick);
+  
+  if (frameData && !frameData->empty()) {
+    // Collect motor commands from frame data
+    std::vector<uint8_t> motorIDs;
+    std::vector<uint16_t> positions;
+
+    for (const auto& motorPos : *frameData) {
+      motorIDs.push_back(motorPos.motorID);
+      positions.push_back(motorPos.position);
+    }
+
+    // Apply motor commands (behaviors will override if needed)
+    applyMotorCommands(motorIDs, positions);
+    
+    // Debug: print frame and motor positions
+    // Serial.print("Frame ");
+    // Serial.print(currentTick);
+    // Serial.print(": ");
+    // for (size_t i = 0; i < motorIDs.size(); i++) {
+    //   if (i > 0) Serial.print(", ");
+    //   Serial.print("M");
+    //   Serial.print(motorIDs[i]);
+    //   Serial.print("=");
+    //   Serial.print(positions[i]);
+    // }
+    // Serial.println();
+  }
+
+  // Emit per-frame event
+  {
+    uint8_t payload[4];
+    payload[0] = currentTick & 0xFF;
+    payload[1] = (currentTick >> 8) & 0xFF;
+    payload[2] = static_cast<uint8_t>(animState.playMode);
+    payload[3] = 0; // in-progress
+    sendPacket(Tag::FRAME, payload, 4);
+  }
+
+  currentTick++;
+
+  // Handle animation end
+  uint16_t totalFrames = animState.current->getFrameCount();
+  uint16_t framesPlayed = currentTick - animState.startFrame;
+  uint16_t remainingFrames = (totalFrames > animState.startFrame) ? (totalFrames - animState.startFrame) : 0;
+
+  // Debug: log completion check near the end
+  if (remainingFrames > 0 && framesPlayed >= remainingFrames - 1) {
+    sendMessage("Completion check - currentTick: " + String(currentTick) + ", framesPlayed: " + String(framesPlayed) + ", remainingFrames: " + String(remainingFrames) + ", totalFrames: " + String(totalFrames));
+  }
+
+  // Check if we've played all remaining frames (from startFrame to totalFrames-1)
+  if (totalFrames > 0 && remainingFrames > 0 && framesPlayed >= remainingFrames) {
+    sendMessage("Animation completion triggered! Sending completion packet.");
+    switch (animState.playMode) {
+      case PLAY_ONCE:
+        {
+          // Send completion packet BEFORE stopping animation
+          uint8_t done[4];
+          done[0] = currentTick & 0xFF;
+          done[1] = (currentTick >> 8) & 0xFF;
+          done[2] = static_cast<uint8_t>(animState.playMode);
+          done[3] = 1; // complete
+          sendPacket(Tag::FRAME, done, 4);
+          animState.stop();
+        }
+        break;
+      case PLAY_LOOP:
+        // Reset to start frame for seamless looping
+        currentTick = animState.startFrame;
+        break;
+      case PLAY_REPEAT:
+        if (--animState.repeatsRemaining == 0) {
+          // Send completion packet BEFORE stopping animation
+          uint8_t done[4];
+          done[0] = currentTick & 0xFF;
+          done[1] = (currentTick >> 8) & 0xFF;
+          done[2] = static_cast<uint8_t>(animState.playMode);
+          done[3] = 1; // complete
+          sendPacket(Tag::FRAME, done, 4);
+          animState.stop();
+  } else {
+          currentTick = animState.startFrame;
+        }
+        break;
+      default:
+        break;
+      }
+  }
+}
+
+// Version 1: Node graph animation playback
+void runNodeAnimation() {
+  static uint32_t lastTickTime = 0;
+  static uint32_t currentTick = 0;
+  static uint8_t lastGeneration = 0;
+
+  if (!animState.current || !animState.current->isActive()) {
+    return;
+  }
+
+  // Reset tick when a new animation starts (detected by generation change)
+  if (lastGeneration != animState.playGeneration) {
+    currentTick = animState.startFrame;  // Start from specified frame
+    lastTickTime = millis();
+    lastGeneration = animState.playGeneration;
+    // Debug: send startFrame via MSGE
+    sendMessage("Animation startFrame: " + String(animState.startFrame) + ", currentTick: " + String(currentTick));
+  }
+
+  config.enableAllMotors();
+
+  // Calculate frame interval from animation's frame rate
+  uint16_t frameIntervalMs = 1000 / animState.current->header.frameRate;
+  if (frameIntervalMs == 0) frameIntervalMs = 1;  // Safety: prevent division by zero
+
+  uint32_t now = millis();
+  if (now - lastTickTime < frameIntervalMs)
+    return;
+    lastTickTime = now;
+
+  // Tick the node graph
+  animState.current->nodeGraph.tick(currentTick, *animState.current);
+  auto outputs = animState.current->nodeGraph.getServoOutputs();
+
+  // Collect motor commands
+    std::vector<uint8_t> motorIDs;
+    std::vector<uint16_t> positions;
+
+  for (const auto &[motorID, value] : outputs) {
+      if (value != 65535) {
+        motorIDs.push_back(motorID);
+        positions.push_back(value);
+  } else {
+      // Only disable torque for motors that should be limp
+        if (config.setMotorEnabled(motorID, false)) {
+        servoManager[0]->disableTorque(motorID);
+      }
+    }
+  }
+
+  // Apply motor commands (behaviors will override if needed)
+  applyMotorCommands(motorIDs, positions);
+
+  // Emit per-frame event: [frameLo, frameHi, playMode, status=0]
+  // Send actual frame number (currentTick), not relative frame
+  {
+    uint8_t payload[4];
+    payload[0] = currentTick & 0xFF;
+    payload[1] = (currentTick >> 8) & 0xFF;
+    payload[2] = static_cast<uint8_t>(animState.playMode);
+    payload[3] = 0; // in-progress
+    sendPacket(Tag::FRAME, payload, 4);
+  }
+
+    currentTick++;
+
+  // Handle animation end (0 = run indefinitely for variable-only animations)
+  // Calculate total frames played: currentTick - startFrame
+  uint16_t framesPlayed = currentTick - animState.startFrame;
+  // Calculate remaining frames: total frames minus startFrame
+  // If animation has 100 frames and we start at 50, we should only play 50 frames
+  uint16_t totalFrames = animState.current->getFrameCount();
+  uint16_t remainingFrames = (totalFrames > animState.startFrame) ? (totalFrames - animState.startFrame) : 0;
+  
+  // Debug: show completion check values every 10 frames
+  if (framesPlayed % 10 == 0 || framesPlayed >= remainingFrames - 1) {
+    sendMessage("Frame check - played: " + String(framesPlayed) + ", remaining: " + String(remainingFrames) + ", total: " + String(totalFrames) + ", startFrame: " + String(animState.startFrame));
+  }
+  
+  if (totalFrames > 0 && remainingFrames > 0 && framesPlayed >= remainingFrames) {
+    switch (animState.playMode) {
+        case PLAY_ONCE:
+      animState.stop();
+        {
+          uint8_t done[4];
+          done[0] = currentTick & 0xFF;
+          done[1] = (currentTick >> 8) & 0xFF;
+          done[2] = static_cast<uint8_t>(animState.playMode);
+          done[3] = 1; // complete
+          sendPacket(Tag::FRAME, done, 4);
+        }
+          break;
+        case PLAY_LOOP:
+      // Reset to start frame for seamless looping
+      currentTick = animState.startFrame;
+      sendMessage("Looping back to startFrame: " + String(animState.startFrame));
+          break;
+        case PLAY_REPEAT:
+      if (--animState.repeatsRemaining == 0) {
+        animState.stop();
+        uint8_t done[4];
+        done[0] = currentTick & 0xFF;
+        done[1] = (currentTick >> 8) & 0xFF;
+        done[2] = static_cast<uint8_t>(animState.playMode);
+        done[3] = 1; // complete
+        sendPacket(Tag::FRAME, done, 4);
+        // Animation stopped, don't reset tick
+      } else {
+        // Reset to start frame for next repeat
+        currentTick = animState.startFrame;
+          }
+          break;
+    default:
+      break;
+    }
+    // Don't reset currentTick here - each case handles it if needed
+  }
+}
+
+// ============================================================================
+// Test Functions
+// ============================================================================
+
+void testSweepMotor40() {
+  static unsigned long lastUpdate = 0;
+  static uint16_t position = 500;
+  static int16_t direction = 1;
+  const unsigned long SWEEP_INTERVAL_MS = 20;  // Update every 20ms
+  const uint16_t MIN_POS = 500;
+  const uint16_t MAX_POS = 2500;
+  const uint16_t STEP = 10;
+  
+  unsigned long now = millis();
+  if (now - lastUpdate < SWEEP_INTERVAL_MS) {
+    return;
+  }
+  lastUpdate = now;
+  
+  // Update position
+  position += (direction * STEP);
+  
+  // Reverse direction at limits
+  if (position >= MAX_POS) {
+    position = MAX_POS;
+    direction = -1;
+  } else if (position <= MIN_POS) {
+    position = MIN_POS;
+    direction = 1;
+  }
+  
+  // Send position to motor 40
+  uint8_t motorID = 40;
+  uint16_t positions[1] = {position};
+  uint16_t speeds[1] = {0};
+  uint8_t ids[1] = {motorID};
+  
+  servoManager.syncWritePositions(ids, positions, speeds, 1, config, 0);
+}
+
+// ============================================================================
+// Motor Position Updates
+// ============================================================================
+
+void updateMotorPositions() {
+  // Only read positions when motor streaming is active
+  // This prevents blocking the main loop when positions aren't needed
+  if (!motorStream.active) {
+    return;
+  }
+
+  static unsigned long lastUpdate = 0;
+
+  if (millis() - lastUpdate < MOTOR_UPDATE_INTERVAL_MS)
+    return;
+  lastUpdate = millis();
+
+  for (const Motor &motor : config.motors) {
+    servoManager[0]->setFeetechMode(motor.servoModel.major);
+    uint16_t position = servoManager[0]->getPosition(motor.motorID);
+    config.setMotorPosition(motor.motorID, position);
+    uint16_t current = servoManager[0]->getCurrent(motor.motorID);
+    config.setMotorCurrent(motor.motorID, current);
+  }
+}
+
+void printMotorStats() {
+  Serial.println("--- Motor stats ---");
+  for (const Motor& motor : config.motors) {
+    Serial.print("ID:");
+    Serial.print(motor.motorID);
+    Serial.print(" pos:");
+    Serial.print(motor.position);
+    Serial.print(" cur:");
+    Serial.println(motor.current);
+  }
+  Serial.println("-------------------");
+}
+
+void handleMotorStreaming() {
+  if (motorStream.shouldStream()) {
+    sendMotorPositions();
+  }
+}
+
+void printLoopRate() {
+  static unsigned long lastPrint = 0;
+  static uint32_t loopCount = 0;
+  
+  loopCount++;
+  
+  unsigned long now = millis();
+  if (now - lastPrint >= 1000) {
+    Serial.print("[Loop] ");
+    Serial.print(loopCount);
+    Serial.println(" Hz");
+    loopCount = 0;
+    lastPrint = now;
+  }
+}
+
+// ============================================================================
+// Heartbeat
+// ============================================================================
+
+void sendHeartbeat() {
+  static unsigned long lastHeartbeat = 0;
+
+  if (millis() - lastHeartbeat < HEARTBEAT_INTERVAL_MS)
+    return;
+  lastHeartbeat = millis();
+
+  // Pack state: uptime(4) + flags(2)
+  uint8_t payload[6];
+  uint32_t uptime = millis() / 1000;
+  uint16_t flags = 0;
+
+  // Build flags
+  if (adxl.isReady())
+    flags |= 0x01;
+  if (animState.current && animState.current->isActive())
+    flags |= 0x02;
+  if (motorStream.active)
+    flags |= 0x04;
+  if (sensors.isADXLStreamEnabled())
+    flags |= 0x08;
+  if (sensors.isRadarStreamEnabled())
+    flags |= 0x10;
+
+  payload[0] = uptime & 0xFF;
+  payload[1] = (uptime >> 8) & 0xFF;
+  payload[2] = (uptime >> 16) & 0xFF;
+  payload[3] = (uptime >> 24) & 0xFF;
+  payload[4] = flags & 0xFF;
+  payload[5] = (flags >> 8) & 0xFF;
+
+  sendPacket(Tag::STATE, payload, 6);
+}
+
+// ============================================================================
+// Setup
+// ============================================================================
+
+void setup() {
+  // Serial setup (buffer size must be set before begin)
+  Serial.setRxBufferSize(8192);
+  Serial.begin(1000000);
+
+  pinMode(6, OUTPUT);
+  digitalWrite(6, 1);
+
+  // Startup delay
+  delay(500);
+  Serial.println("\n[HansonServo] Starting...");
+
+  // Initialize servo manager
+  servoManager.init();
+  Serial.println("[HansonServo] Servos initialized");
+
+  // Initialize sensors
+  sensors.init();
+
+  // Initialize filesystem
+  if (!FFat.begin(true)) {
+    Serial.println("[HansonServo] FFat mount failed!");
+      return;
+    }
+  Serial.println("[HansonServo] Filesystem ready");
+
+  
+
+  // Initialize behaviors (order determines priority: first added = highest priority)
+  // Priority: Focus > Viseme > Idle
+  // NOTE: Don't set enabled state here - let config load restore it, or set defaults after
+  
+  // 1. Focus behavior (highest priority - radar tracking)
+  static FocusBehavior focusBehavior;
+  behaviorManager.addBehavior(BEHAVIOR_FOCUS, &focusBehavior);
+  
+  // 2. Viseme behavior (medium priority - mouth animation for speech)
+  behaviorManager.addBehavior(BEHAVIOR_VISEME, &visemeBehavior);
+  
+  // 3. Idle behavior (lowest priority - perlin noise for all motors)
+  static IdleBehavior idleBehavior;
+  behaviorManager.addBehavior(BEHAVIOR_IDLE, &idleBehavior);
+  
+  Serial.println("[HansonServo] Behaviors initialized (focus > viseme > idle)");
+
+  // Check if config file exists before loading
+  bool configExisted = FFat.exists("/robot_config.bin");
+  
+  // Load full config with behaviors and visemes (will restore their state)
+  // This must happen BEFORE setting defaults, so saved states aren't overwritten
+  bool configLoaded = config.loadOrCreateDefault("/robot_config.bin", &behaviorManager, &visemeBehavior);
+  
+  if (configLoaded) {
+    Serial.println("[HansonServo] Config loaded: " + config.deviceName);
+    
+    // If config file existed before, behavior states should have been loaded
+    // If it's a new file, we need to set defaults
+    if (!configExisted) {
+      Serial.println("[HansonServo] New config file, setting default behavior states...");
+      behaviorManager.setBehaviorEnabled(BEHAVIOR_FOCUS, true);
+      behaviorManager.setBehaviorEnabled(BEHAVIOR_VISEME, true);
+      behaviorManager.setBehaviorEnabled(BEHAVIOR_IDLE, true);
+      // Save the defaults
+      config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior);
+    } else {
+      Serial.println("[HansonServo] Behavior states loaded from config");
+    }
+    
+    // Check if visemes were loaded from config
+    if (visemeBehavior.getVisemes().empty()) {
+      Serial.println("[HansonServo] No visemes in config, creating defaults...");
+      // Only create defaults if config had no visemes
+      visemeBehavior.addViseme(0, "SIL", 2047, 2047, 2047);  // Neutral/rest (sil)
+      visemeBehavior.addViseme(1, "AA ", 2200, 1900, 2100);  // AA (as in "father")
+      visemeBehavior.addViseme(2, "AE ", 2100, 2000, 2000);  // AE (as in "cat")
+      visemeBehavior.addViseme(3, "AH ", 2150, 1950, 2050);  // AH (as in "but")
+      visemeBehavior.addViseme(4, "AO ", 2000, 2100, 1950);  // AO (as in "bought")
+      visemeBehavior.addViseme(5, "EH ", 1900, 2200, 1900);  // EH (as in "bed")
+      visemeBehavior.addViseme(6, "IH ", 1850, 2250, 1850);  // IH (as in "bit")
+      visemeBehavior.addViseme(7, "IY ", 1800, 2300, 1800);  // IY (as in "beat")
+      visemeBehavior.addViseme(8, "OW ", 2300, 1800, 2200);  // OW (as in "boat")
+      visemeBehavior.addViseme(9, "UH ", 2250, 1850, 2150);  // UH (as in "book")
+      visemeBehavior.addViseme(10, "UW ", 2200, 1900, 2100); // UW (as in "boot")
+      // Save the defaults
+      config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior);
+    } else {
+      Serial.println("[HansonServo] Visemes loaded from config: " + String(visemeBehavior.getVisemes().size()));
+    }
+  } else {
+    Serial.println("[HansonServo] Config init failed");
+    // Set defaults on failure
+    behaviorManager.setBehaviorEnabled(BEHAVIOR_FOCUS, true);
+    behaviorManager.setBehaviorEnabled(BEHAVIOR_VISEME, true);
+    behaviorManager.setBehaviorEnabled(BEHAVIOR_IDLE, true);
+  }
+  
+  // Initialize idle behavior motors (needs config.motors to be loaded)
+  std::vector<uint8_t> allMotorIDs;
+  for (const Motor& motor : config.motors) {
+    allMotorIDs.push_back(motor.motorID);
+  }
+  idleBehavior.initMotors(allMotorIDs);
+
+  motorStream.start();
+  
+  // Initialize motor-aided movement
+  // Motor 25 - default settings
+  motorAid.addMotor(25);
+  
+  // Motors 30, 31, 35, 36 - high gear ratio, maximum sensitivity
+  AidedMotorSettings highGearSettings;
+  highGearSettings.velocityThreshold = 8;    // Extremely sensitive trigger
+  highGearSettings.resetThreshold = 4;       // Minimal hysteresis
+  highGearSettings.leadOffset = 100;         // Smaller lead for fine control
+  highGearSettings.assistSpeed = 600;        // Moderate speed
+  highGearSettings.assistDuration = 400;     // Shorter burst
+  
+  motorAid.addMotor(30, highGearSettings);
+  motorAid.addMotor(31, highGearSettings);
+  motorAid.addMotor(35, highGearSettings);
+  motorAid.addMotor(36, highGearSettings);
+
+  Serial.println("[HansonServo] Ready");
+  Serial.println("[HansonServo] Protocol: 0xA5 0x5A tagged packets with CRC16");
+
+  // ---- TEST: Load and play animation ----
+  // Serial.println("[TEST] Loading /slow.anim...");
+  // if (animState.animation.loadFromFile("/slow.anim")) {
+  //   Serial.println("[TEST] Animation loaded successfully");
+    
+  //   delay(1000);  // Wait 1 second
+    
+  //   // Print animation info
+  //   Serial.println(animState.animation.printAnim());
+    
+  //   delay(5000);  // Wait 5 seconds
+    
+  //   // Play the animation
+  //   Serial.println("[TEST] Playing animation...");
+  //   animState.play(PLAY_ONCE, 1, 0);
+  // } else {
+  //   Serial.println("[TEST] Failed to load /animation.anim");
+  // }
+  // ---- END TEST ----
+}
+
+// ============================================================================
+// Main Loop
+// ============================================================================
+
+void loop() {
+// Serial passthrough (when enabled)
+#if ENABLE_SERIAL_PASSTHROUGH
+  handleSerialPassthrough();
+    return;
+#endif
+
+  // Protocol handling
+  handleProtocol();
+
+  // Update behaviors
+  behaviorManager.update();
+
+  // Animation playback (auto-selects v1 node or v2 frame based on version)
+  runAnimation();
+  
+  // If no animation is active, still apply behavior motor commands
+  if (!animState.current || !animState.current->isActive()) {
+    applyBehaviorMotorCommands();
+  }
+
+  // Motor position updates
+  updateMotorPositions();
+  handleMotorStreaming();
+  
+  // Motor-aided movement (when motorStream is active)
+  if (motorStream.active) {
+    motorAid.update();
+  }
+  //printMotorStats();
+  // Sensor updates and streaming
+  //sensors.update();
+
+  // Heartbeat
+  //sendHeartbeat();
+  
+  // Debug: print loop rate
+  printLoopRate();
+  
+  // ============================================================================
+  // TEST: Uncomment to enable motor 40 sweep test
+  // ============================================================================
+   //testSweepMotor40();
+}

motoraid.cpp

@@ -0,0 +1,221 @@
+#include "motoraid.h"
+#include "motorcontrol.h"
+#include "robotconfig.h"
+
+// Global instance
+MotorAid motorAid;
+
+extern RobotConfig config;
+
+void MotorAid::addMotor(uint8_t motorID) {
+  // Use default settings
+  AidedMotorSettings defaultSettings;
+  addMotor(motorID, defaultSettings);
+}
+
+void MotorAid::addMotor(uint8_t motorID, const AidedMotorSettings& settings) {
+  // Don't add duplicates
+  if (findMotor(motorID) != nullptr) return;
+  
+  AidedMotor motor;
+  motor.motorID = motorID;
+  motor.settings = settings;
+  
+  // Read actual position directly from servo (not config which may be stale)
+  uint8_t model = config.getMotorModel(motorID);
+  servoManager[0]->setFeetechMode(model);
+  motor.lastPosition = servoManager[0]->getPosition(motorID);
+  motor.stablePosition = motor.lastPosition;  // Initialize stable position
+  motor.lastUpdateTime = millis();
+  
+  // Initialize velocity samples to zero
+  for (uint8_t i = 0; i < VELOCITY_SAMPLES; i++) {
+    motor.velocitySamples[i] = 0;
+  }
+  
+  aidedMotors.push_back(motor);
+  
+  // Disable torque on this motor so it can be moved freely
+  servoManager[0]->disableTorque(motorID);
+  
+  Serial.print("[MotorAid] Added motor ");
+  Serial.print(motorID);
+  Serial.print(" at pos ");
+  Serial.print(motor.lastPosition);
+  Serial.print(" (thresh=");
+  Serial.print(settings.velocityThreshold);
+  Serial.print(", lead=");
+  Serial.print(settings.leadOffset);
+  Serial.println(")");
+}
+
+void MotorAid::removeMotor(uint8_t motorID) {
+  for (auto it = aidedMotors.begin(); it != aidedMotors.end(); ++it) {
+    if (it->motorID == motorID) {
+      aidedMotors.erase(it);
+      Serial.print("[MotorAid] Removed motor ");
+      Serial.println(motorID);
+      return;
+    }
+  }
+}
+
+bool MotorAid::isMotorAided(uint8_t motorID) const {
+  return findMotor(motorID) != nullptr;
+}
+
+AidedMotorSettings* MotorAid::getMotorSettings(uint8_t motorID) {
+  AidedMotor* motor = findMotor(motorID);
+  if (motor) return &motor->settings;
+  return nullptr;
+}
+
+AidedMotor* MotorAid::findMotor(uint8_t motorID) {
+  for (auto& motor : aidedMotors) {
+    if (motor.motorID == motorID) return &motor;
+  }
+  return nullptr;
+}
+
+const AidedMotor* MotorAid::findMotor(uint8_t motorID) const {
+  for (const auto& motor : aidedMotors) {
+    if (motor.motorID == motorID) return &motor;
+  }
+  return nullptr;
+}
+
+int16_t MotorAid::calculateSmoothedVelocity(AidedMotor& motor) {
+  // Calculate average of velocity samples
+  int32_t sum = 0;
+  for (uint8_t i = 0; i < VELOCITY_SAMPLES; i++) {
+    sum += motor.velocitySamples[i];
+  }
+  return (int16_t)(sum / VELOCITY_SAMPLES);
+}
+
+void MotorAid::update() {
+  unsigned long now = millis();
+  
+  for (AidedMotor& motor : aidedMotors) {
+    const AidedMotorSettings& s = motor.settings;  // Shorthand
+    
+    // Calculate time delta
+    unsigned long deltaTime = now - motor.lastUpdateTime;
+    if (deltaTime < updateInterval) continue;
+    
+    // Get current position from config (already updated by updateMotorPositions)
+    uint16_t currentPosition = config.getMotorPosition(motor.motorID);
+    
+    // Calculate position delta from stable position (filters noise)
+    int16_t positionDelta = (int16_t)currentPosition - (int16_t)motor.stablePosition;
+    
+    // Only register movement if it exceeds minimum delta (noise filter)
+    int16_t instantVelocity = 0;
+    if (abs(positionDelta) >= MIN_POSITION_DELTA) {
+      // Significant movement - calculate velocity and update stable position
+      instantVelocity = (positionDelta * 1000) / (int16_t)deltaTime;
+      motor.stablePosition = currentPosition;
+    }
+    // else: tiny movement, treat as zero velocity (noise)
+    
+    // Add to velocity samples (circular buffer)
+    motor.velocitySamples[motor.sampleIndex] = instantVelocity;
+    motor.sampleIndex = (motor.sampleIndex + 1) % VELOCITY_SAMPLES;
+    
+    // Track samples collected for warmup
+    if (motor.samplesCollected < VELOCITY_SAMPLES * 2) {
+      motor.samplesCollected++;
+    }
+    
+    // Don't calculate or trigger until we have enough samples (2x buffer = ~300ms warmup)
+    if (motor.samplesCollected < VELOCITY_SAMPLES * 2) {
+      continue;  // Still warming up
+    }
+    motor.warmedUp = true;
+    
+    // Calculate smoothed velocity
+    motor.smoothedVelocity = calculateSmoothedVelocity(motor);
+    
+    // Update tracking
+    motor.lastUpdateTime = now;
+    
+    // Check if currently assisting
+    if (motor.assisting) {
+      // Calculate target position AHEAD of current position in direction of movement
+      int16_t leadOffset = (motor.smoothedVelocity > 0) ? s.leadOffset : -s.leadOffset;
+      motor.assistTargetPosition = constrain((int16_t)currentPosition + leadOffset, 0, 4095);
+      
+      // Send updated position command to lead the movement
+      uint8_t model = config.getMotorModel(motor.motorID);
+      servoManager[0]->setFeetechMode(model);
+      
+      uint8_t ids[1] = { motor.motorID };
+      uint16_t positions[1] = { motor.assistTargetPosition };
+      uint16_t speeds[1] = { s.assistSpeed };
+      servoManager.syncWritePositions(ids, positions, speeds, 1, config, 0);
+      
+      // Check if assist duration has elapsed (fixed duration, always ends)
+      if (now - motor.assistStartTime >= s.assistDuration) {
+        motor.assisting = false;
+        // Disable torque again
+        servoManager[0]->disableTorque(motor.motorID);
+        
+        Serial.print("[MotorAid] Assist ended, motor ");
+        Serial.print(motor.motorID);
+        Serial.print(" at pos ");
+        Serial.println(currentPosition);
+      }
+      continue;
+    }
+    
+    // Hysteresis: if we were triggered, wait for velocity to drop before re-triggering
+    if (motor.wasTriggered) {
+      if (abs(motor.smoothedVelocity) < s.resetThreshold) {
+        motor.wasTriggered = false;  // Reset, can trigger again
+        motor.consecutiveHighCount = 0;  // Reset consecutive counter too
+      }
+      continue;  // Don't trigger while in hysteresis zone
+    }
+    
+    // Check if smoothed velocity exceeds threshold (in either direction)
+    if (abs(motor.smoothedVelocity) > s.velocityThreshold) {
+      motor.consecutiveHighCount++;
+    } else {
+      motor.consecutiveHighCount = 0;  // Reset on low reading
+    }
+    
+    // Only trigger after CONSECUTIVE_REQUIRED high readings (filters noise spikes)
+    if (motor.consecutiveHighCount >= CONSECUTIVE_REQUIRED) {
+      // Sustained movement detected - assist!
+      motor.assisting = true;
+      motor.assistStartTime = now;
+      motor.wasTriggered = true;
+      
+      // Calculate target position AHEAD of current position in direction of movement
+      int16_t leadOffset = (motor.smoothedVelocity > 0) ? s.leadOffset : -s.leadOffset;
+      motor.assistTargetPosition = constrain((int16_t)currentPosition + leadOffset, 0, 4095);
+      
+      // Set servo mode based on motor model
+      uint8_t model = config.getMotorModel(motor.motorID);
+      servoManager[0]->setFeetechMode(model);
+      
+      // Enable torque and move AHEAD to assist
+      servoManager[0]->enableTorque(motor.motorID);
+      
+      uint8_t ids[1] = { motor.motorID };
+      uint16_t positions[1] = { motor.assistTargetPosition };
+      uint16_t speeds[1] = { s.assistSpeed };
+      servoManager.syncWritePositions(ids, positions, speeds, 1, config, 0);
+      
+      // Debug output
+      Serial.print("[MotorAid] Assisting motor ");
+      Serial.print(motor.motorID);
+      Serial.print(" vel=");
+      Serial.print(motor.smoothedVelocity);
+      Serial.print(" pos=");
+      Serial.print(currentPosition);
+      Serial.print(" -> ");
+      Serial.println(motor.assistTargetPosition);
+    }
+  }
+}

motoraid.h

@@ -0,0 +1,82 @@
+#pragma once
+#include <Arduino.h>
+#include <vector>
+
+// Motor-aided movement: detects external manipulation and assists to reduce gear stress
+// Works by monitoring position velocity - when someone moves the motor manually,
+// it briefly enables torque and moves to the current position to assist.
+
+constexpr uint8_t VELOCITY_SAMPLES = 4;  // Number of samples for velocity smoothing (fewer = faster response)
+constexpr int16_t MIN_POSITION_DELTA = 1; // Ignore position changes smaller than this (minimum for high gear ratio)
+constexpr uint8_t CONSECUTIVE_REQUIRED = 3; // Require this many consecutive high readings to trigger
+
+struct AidedMotorSettings {
+  int16_t velocityThreshold = 80;      // Position units/sec to trigger assist (low for high gear ratio)
+  int16_t resetThreshold = 30;         // Velocity must drop below this to re-trigger
+  int16_t leadOffset = 200;            // How far ahead to command (position units)
+  uint16_t assistSpeed = 1000;         // Speed to use when assisting
+  unsigned long assistDuration = 500;  // ms to assist for
+};
+
+struct AidedMotor {
+  uint8_t motorID;
+  uint16_t lastPosition = 0;
+  unsigned long lastUpdateTime = 0;
+  
+  // Per-motor settings
+  AidedMotorSettings settings;
+  
+  // Velocity smoothing
+  int16_t velocitySamples[VELOCITY_SAMPLES] = {0};
+  uint8_t sampleIndex = 0;
+  uint8_t samplesCollected = 0;  // Track how many samples we've collected
+  int16_t smoothedVelocity = 0;
+  uint16_t stablePosition = 0;   // Position used for delta calculation (filters noise)
+  
+  // Assist state
+  bool assisting = false;
+  unsigned long assistStartTime = 0;
+  uint16_t assistTargetPosition = 0;
+  
+  // Hysteresis - prevents re-triggering until velocity drops
+  bool wasTriggered = false;
+  
+  // Warmup - don't trigger until we have enough samples
+  bool warmedUp = false;
+  
+  // Consecutive high readings counter (filters noise spikes)
+  uint8_t consecutiveHighCount = 0;
+};
+
+class MotorAid {
+public:
+  // Add a motor with default settings
+  void addMotor(uint8_t motorID);
+  
+  // Add a motor with custom settings
+  void addMotor(uint8_t motorID, const AidedMotorSettings& settings);
+  
+  // Remove a motor from aided list
+  void removeMotor(uint8_t motorID);
+  
+  // Check if a motor is being aided
+  bool isMotorAided(uint8_t motorID) const;
+  
+  // Get settings for a motor (to modify)
+  AidedMotorSettings* getMotorSettings(uint8_t motorID);
+  
+  // Main update function - call from loop() when motorStream is active
+  void update();
+  
+private:
+  std::vector<AidedMotor> aidedMotors;
+  unsigned long updateInterval = 30;   // ms between position checks
+  
+  AidedMotor* findMotor(uint8_t motorID);
+  const AidedMotor* findMotor(uint8_t motorID) const;
+  
+  // Calculate smoothed velocity from samples
+  int16_t calculateSmoothedVelocity(AidedMotor& motor);
+};
+
+extern MotorAid motorAid;

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

@@ -1,10 +1,13 @@
 #include "nodegraph.h"
 #include "animation.h"
 #include "noise.h"
-#include "RobotConfig.h"
+#include "robotconfig.h"
+#include "sensors.h"
 #include <cstring>
 
 extern RobotConfig config;
+extern ADXL345 adxl;
+extern Radar radar;
 
 
 // CurveNode evaluation
@@ -61,6 +64,79 @@ void VariableNode::evaluate(uint32_t currentTick) {
     case VAR_SERVO_FEEDBACK:
       outputValue = config.getMotorPosition(arg0);
       break;
+      
+    // ADXL345 accelerometer sources - scale to 0-4095 range
+    case VAR_ACCEL_X:
+      // X acceleration: -2g to +2g → 0-4095 (2048 = 0g)
+      if (adxl.isReady()) {
+        float accel = constrain(adxl.getAccelX(), -2.0f, 2.0f);
+        outputValue = (uint16_t)(((accel + 2.0f) / 4.0f) * 4095.0f);
+      } else {
+        outputValue = 2048;
+      }
+      break;
+    case VAR_ACCEL_Y:
+      // Y acceleration: -2g to +2g → 0-4095 (2048 = 0g)
+      if (adxl.isReady()) {
+        float accel = constrain(adxl.getAccelY(), -2.0f, 2.0f);
+        outputValue = (uint16_t)(((accel + 2.0f) / 4.0f) * 4095.0f);
+      } else {
+        outputValue = 2048;
+      }
+      break;
+    case VAR_ACCEL_Z:
+      // Z acceleration: -2g to +2g → 0-4095 (2048 = 0g)
+      if (adxl.isReady()) {
+        float accel = constrain(adxl.getAccelZ(), -2.0f, 2.0f);
+        outputValue = (uint16_t)(((accel + 2.0f) / 4.0f) * 4095.0f);
+      } else {
+        outputValue = 2048;
+      }
+      break;
+    case VAR_TILT_PITCH:
+      // Pitch angle: -90° to +90° → 0-4095 (2048 = level)
+      if (adxl.isReady()) {
+        float pitch = constrain(adxl.getPitch(), -90.0f, 90.0f);
+        outputValue = (uint16_t)(((pitch + 90.0f) / 180.0f) * 4095.0f);
+      } else {
+        outputValue = 2048;
+      }
+      break;
+    case VAR_TILT_ROLL:
+      // Roll angle: -90° to +90° → 0-4095 (2048 = level)
+      if (adxl.isReady()) {
+        float roll = constrain(adxl.getRoll(), -90.0f, 90.0f);
+        outputValue = (uint16_t)(((roll + 90.0f) / 180.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,16 @@ enum VariableSource {
   VAR_FACE_Y,
   VAR_SINE,
   VAR_ANALOG,
-  VAR_SERVO_FEEDBACK
+  VAR_SERVO_FEEDBACK,
+  // ADXL345 accelerometer sources
+  VAR_ACCEL_X,        // X acceleration → 0-4095 (-2g to +2g, 2048 = 0g)
+  VAR_ACCEL_Y,        // Y acceleration → 0-4095 (-2g to +2g, 2048 = 0g)
+  VAR_ACCEL_Z,        // Z acceleration → 0-4095 (-2g to +2g, 2048 = 0g)
+  VAR_TILT_PITCH,     // Pitch angle from accel → 0-4095 (-90° to +90°, 2048 = level)
+  VAR_TILT_ROLL,      // Roll angle from accel → 0-4095 (-90° to +90°, 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,131 @@
+#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
+  constexpr char FSTP[4]  = {'F','S','T','P'};  // Stop animation
+  
+  // 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'};  // ADXL acceleration data (x,y,z)
+  constexpr char RADAR[4] = {'R','D','A','R'};  // Radar targets
+  constexpr char FRAME[4] = {'F','R','M','E'};  // Animation frame events (frame, mode, status)
+  
+  // Behaviors
+  constexpr char BHVR[4]  = {'B','H','V','R'};  // Behavior control (activate/deactivate)
+  constexpr char BLST[4]  = {'B','L','S','T'};  // Behavior list (list all behaviors and their states)
+  
+  // Visemes
+  constexpr char VLST[4]  = {'V','L','S','T'};  // List all visemes
+  constexpr char VADD[4]  = {'V','A','D','D'};  // Add a new viseme
+  constexpr char VDEL[4]  = {'V','D','E','L'};  // Delete a viseme
+  constexpr char VSET[4]  = {'V','S','E','T'};  // Set viseme motor positions
+  constexpr char VSME[4]  = {'V','S','M','E'};  // Trigger a viseme by ID
+  
+  // 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;
+}

robotconfig.cpp

@@ -1,4 +1,5 @@
-#include "RobotConfig.h"
+#include "robotconfig.h"
+#include "behaviors.h"
 #include <FFat.h>
 
 uint16_t RobotConfig::getMotorPosition(uint8_t motorID) const {
@@ -11,6 +12,15 @@ uint16_t RobotConfig::getMotorPosition(uint8_t motorID) const {
   return 2047;
 }
 
+uint16_t RobotConfig::getMotorCurrent(uint8_t motorID) const {
+  for (const Motor& motor : motors) {
+    if (motor.motorID == motorID) {
+      return motor.current;
+    }
+  }
+  return 0;
+}
+
 uint8_t RobotConfig::getMotorModel(uint8_t motorID) {
   for (const Motor& motor : motors) {
     if (motor.motorID == motorID) {
@@ -32,6 +42,16 @@ bool RobotConfig::setMotorPosition(uint8_t motorID, uint16_t newPosition) {
   return false;
 }
 
+bool RobotConfig::setMotorCurrent(uint8_t motorID, uint16_t newCurrent) {
+  for (Motor& motor : motors) {
+    if (motor.motorID == motorID) {
+      motor.current = newCurrent;
+      return true;
+    }
+  }
+  return false;
+}
+
 bool RobotConfig::setMotorEnabled(uint8_t motorID, bool enable) {
   for (Motor& motor : motors) {
     if (motor.motorID == motorID) {
@@ -198,10 +218,310 @@ bool RobotConfig::loadFromFFat(const char* path) {
   return true;
 }
 
+// Legacy method - calls new version with null pointers
 bool RobotConfig::loadOrCreateDefault(const char* path) {
+  return loadOrCreateDefault(path, nullptr, nullptr);
+}
+
+// ============================================================================
+// Key-Value Format V2 (Compact, Extensible)
+// ============================================================================
+
+bool RobotConfig::saveToFFatV2(const char* path, BehaviorManager* behaviorManager, VisemeBehavior* visemeBehavior) const {
+  File file = FFat.open(path, FILE_WRITE);
+  if (!file) return false;
+
+  // Write version header
+  file.write((uint8_t)0x02);  // Version 2
+
+  // Count settings (we'll write this after we know the count)
+  uint16_t settingCount = 0;
+  size_t countPos = file.position();
+  file.write((uint8_t)0);  // Placeholder for count low byte
+  file.write((uint8_t)0);  // Placeholder for count high byte
+
+  // Setting 1: Device Name
+  if (deviceName.length() > 0) {
+    file.write((uint8_t)(KEY_DEVICE_NAME & 0xFF));
+    file.write((uint8_t)((KEY_DEVICE_NAME >> 8) & 0xFF));
+    file.write(TYPE_STRING);
+    uint8_t nameLen = deviceName.length();
+    file.write(nameLen);
+    file.write((const uint8_t*)deviceName.c_str(), nameLen);
+    settingCount++;
+  }
+
+  // Setting 2: Firmware Major
+  file.write((uint8_t)(KEY_FIRMWARE_MAJOR & 0xFF));
+  file.write((uint8_t)((KEY_FIRMWARE_MAJOR >> 8) & 0xFF));
+  file.write(TYPE_UINT8);
+  file.write(firmwareVersion.major);
+  settingCount++;
+
+  // Setting 3: Firmware Minor
+  file.write((uint8_t)(KEY_FIRMWARE_MINOR & 0xFF));
+  file.write((uint8_t)((KEY_FIRMWARE_MINOR >> 8) & 0xFF));
+  file.write(TYPE_UINT8);
+  file.write(firmwareVersion.minor);
+  settingCount++;
+
+  // Setting 4: Motor Array
+  if (!motors.empty()) {
+    file.write((uint8_t)(KEY_MOTOR_ARRAY & 0xFF));
+    file.write((uint8_t)((KEY_MOTOR_ARRAY >> 8) & 0xFF));
+    file.write(TYPE_MOTOR_ARRAY);
+    
+    uint8_t motorCount = motors.size();
+    file.write(motorCount);
+    
+    for (const Motor& m : motors) {
+      file.write(m.motorID);
+      file.write(m.servoModel.major);
+      file.write(m.servoModel.minor);
+      
+      uint8_t nameLen = m.name.length();
+      file.write(nameLen);
+      if (nameLen > 0) {
+        file.write((const uint8_t*)m.name.c_str(), nameLen);
+      }
+    }
+    settingCount++;
+  }
+
+  // Setting 5: Behavior States
+  if (behaviorManager) {
+    file.write((uint8_t)(KEY_BEHAVIOR_STATES & 0xFF));
+    file.write((uint8_t)((KEY_BEHAVIOR_STATES >> 8) & 0xFF));
+    file.write(TYPE_BEHAVIOR_STATES);
+    
+    // Count enabled behaviors
+    uint8_t behaviorCount = behaviorManager->getBehaviorCount();
+    file.write(behaviorCount);
+    
+    // Write behaviorID + enabled state pairs
+    for (uint8_t i = 0; i < behaviorCount; i++) {
+      BehaviorID id;
+      bool enabled;
+      if (behaviorManager->getBehaviorInfo(i, id, enabled)) {
+        file.write((uint8_t)id);
+        file.write(enabled ? 1 : 0);
+      }
+    }
+    settingCount++;
+  }
+
+  // Setting 6: Viseme Array
+  if (visemeBehavior) {
+    const std::vector<Viseme>& visemes = visemeBehavior->getVisemes();
+    if (!visemes.empty()) {
+      file.write((uint8_t)(KEY_VISEME_ARRAY & 0xFF));
+      file.write((uint8_t)((KEY_VISEME_ARRAY >> 8) & 0xFF));
+      file.write(TYPE_VISEME_ARRAY);
+      
+      uint8_t visemeCount = visemes.size();
+      file.write(visemeCount);
+      
+      for (const Viseme& v : visemes) {
+        file.write(v.id);
+        
+        // Label (3 bytes)
+        file.write((uint8_t)v.label[0]);
+        file.write((uint8_t)v.label[1]);
+        file.write((uint8_t)v.label[2]);
+        
+        // Motor positions
+        uint8_t motorCount = v.motorPositions.size();
+        file.write(motorCount);
+        
+        for (const VisemeMotorPosition& mp : v.motorPositions) {
+          file.write(mp.motorID);
+          file.write((uint8_t)(mp.position & 0xFF));         // position low
+          file.write((uint8_t)((mp.position >> 8) & 0xFF));  // position high
+        }
+      }
+      settingCount++;
+    }
+  }
+
+  // Write setting count at the beginning
+  size_t endPos = file.position();
+  file.seek(countPos);
+  file.write((uint8_t)(settingCount & 0xFF));
+  file.write((uint8_t)((settingCount >> 8) & 0xFF));
+  file.seek(endPos);
+
+  file.close();
+  return true;
+}
+
+bool RobotConfig::loadFromFFatV2(const char* path, BehaviorManager* behaviorManager, VisemeBehavior* visemeBehavior) {
+  File file = FFat.open(path, FILE_READ);
+  if (!file) return false;
+
+  // Read version
+  uint8_t version = file.read();
+  if (version != 0x02) {
+    file.close();
+    return false;  // Wrong version
+  }
+
+  // Read setting count
+  uint16_t settingCount = file.read();
+  settingCount |= (file.read() << 8);
+
+  motors.clear();
+
+  // Parse each setting
+  for (uint16_t i = 0; i < settingCount; i++) {
+    // Read key
+    uint16_t key = file.read();
+    key |= (file.read() << 8);
+    
+    // Read type
+    uint8_t type = file.read();
+
+    switch (key) {
+      case KEY_DEVICE_NAME: {
+        if (type == TYPE_STRING) {
+          uint8_t nameLen = file.read();
+          char nameBuf[64] = {0};
+          if (nameLen > 0 && nameLen < sizeof(nameBuf)) {
+            file.readBytes(nameBuf, nameLen);
+            deviceName = String(nameBuf);
+          }
+        }
+        break;
+      }
+
+      case KEY_FIRMWARE_MAJOR: {
+        if (type == TYPE_UINT8) {
+          firmwareVersion.major = file.read();
+        }
+        break;
+      }
+
+      case KEY_FIRMWARE_MINOR: {
+        if (type == TYPE_UINT8) {
+          firmwareVersion.minor = file.read();
+        }
+        break;
+      }
+
+      case KEY_MOTOR_ARRAY: {
+        if (type == TYPE_MOTOR_ARRAY) {
+          uint8_t motorCount = file.read();
+          
+          for (uint8_t j = 0; j < motorCount; j++) {
+            Motor m;
+            m.motorID = file.read();
+            m.servoModel.major = file.read();
+            m.servoModel.minor = file.read();
+            
+            uint8_t nameLen = file.read();
+            char nameBuf[64] = {0};
+            if (nameLen > 0 && nameLen < sizeof(nameBuf)) {
+              file.readBytes(nameBuf, nameLen);
+              m.name = String(nameBuf);
+            }
+            
+            m.position = 0;
+            m.isEnabled = true;
+            motors.push_back(m);
+          }
+        }
+        break;
+      }
+
+      case KEY_BEHAVIOR_STATES: {
+        if (type == TYPE_BEHAVIOR_STATES && behaviorManager) {
+          uint8_t behaviorCount = file.read();
+          
+          for (uint8_t j = 0; j < behaviorCount; j++) {
+            BehaviorID id = (BehaviorID)file.read();
+            bool enabled = file.read() != 0;
+            behaviorManager->setBehaviorEnabled(id, enabled);
+          }
+        }
+        break;
+      }
+
+      case KEY_VISEME_ARRAY: {
+        if (type == TYPE_VISEME_ARRAY && visemeBehavior) {
+          uint8_t visemeCount = file.read();
+          
+          for (uint8_t j = 0; j < visemeCount; j++) {
+            uint8_t visemeID = file.read();
+            
+            // Read label (3 bytes)
+            char label[4];
+            label[0] = file.read();
+            label[1] = file.read();
+            label[2] = file.read();
+            label[3] = '\0';
+            
+            // Read motor positions
+            uint8_t motorCount = file.read();
+            std::vector<VisemeMotorPosition> positions;
+            
+            for (uint8_t k = 0; k < motorCount; k++) {
+              VisemeMotorPosition mp;
+              mp.motorID = file.read();
+              uint16_t posLow = file.read();
+              uint16_t posHigh = file.read();
+              mp.position = posLow | (posHigh << 8);
+              positions.push_back(mp);
+            }
+            
+            // Create or update viseme
+            visemeBehavior->createOrUpdateViseme(visemeID, label, positions);
+          }
+        }
+        break;
+      }
+
+      default:
+        // Unknown key - skip based on type
+        switch (type) {
+          case TYPE_UINT8: file.read(); break;
+          case TYPE_UINT16: file.read(); file.read(); break;
+          case TYPE_UINT32: file.read(); file.read(); file.read(); file.read(); break;
+          case TYPE_BOOL: file.read(); break;
+          case TYPE_INT8: file.read(); break;
+          case TYPE_INT16: file.read(); file.read(); break;
+          case TYPE_INT32: file.read(); file.read(); file.read(); file.read(); break;
+          case TYPE_FLOAT: file.read(); file.read(); file.read(); file.read(); break;
+          case TYPE_STRING: {
+            uint8_t len = file.read();
+            for (uint8_t k = 0; k < len; k++) file.read();
+            break;
+          }
+          default:
+            file.close();
+            return false;  // Unknown type
+        }
+        break;
+    }
+  }
+
+  file.close();
+  return true;
+}
+
+bool RobotConfig::loadOrCreateDefault(const char* path, BehaviorManager* behaviorManager, VisemeBehavior* visemeBehavior) {
   if (FFat.exists(path)) {
     Serial.println("Loading robot config from FFat...");
-    return loadFromFFat(path);
+    // Try V2 format first
+    if (loadFromFFatV2(path, behaviorManager, visemeBehavior)) {
+      Serial.println("Loaded V2 format");
+      return true;
+    }
+    // Fall back to V1 format
+    if (loadFromFFat(path)) {
+      Serial.println("Loaded V1 format (legacy)");
+      // Upgrade to V2 format
+      saveToFFatV2(path, behaviorManager, visemeBehavior);
+      return true;
+    }
   }
 
   Serial.println("No config found. Creating default config...");
@@ -209,8 +529,7 @@ bool RobotConfig::loadOrCreateDefault(const char* path) {
   // 🔧 Define your default config here
   deviceName = "DefaultBot";
   firmwareVersion = { 1, 0 };
-
   motors.clear();
 
-  return saveToFFat(path);
+  return saveToFFatV2(path, behaviorManager, visemeBehavior);
 }

robotconfig.h

@@ -2,6 +2,50 @@
 #include <Arduino.h>
 #include <vector>
 
+// Forward declarations
+class BehaviorManager;
+class VisemeBehavior;
+
+// ============================================================================
+// Config Key-Value System
+// ============================================================================
+
+enum ConfigKey : uint16_t {
+  // Basic settings
+  KEY_DEVICE_NAME = 0x0001,
+  KEY_FIRMWARE_MAJOR = 0x0002,
+  KEY_FIRMWARE_MINOR = 0x0003,
+  
+  // Motor array (single entry containing all motors)
+  KEY_MOTOR_ARRAY = 0x0100,
+  
+  // Behavior states (array of behaviorID + enabled pairs)
+  KEY_BEHAVIOR_STATES = 0x0200,
+  
+  // Viseme array (single entry containing all visemes)
+  KEY_VISEME_ARRAY = 0x0300,
+  
+  // Future extensible settings
+  KEY_SERIAL_BAUD = 0x0400,
+  KEY_MOTOR_UPDATE_INTERVAL = 0x0401,
+  // ... add more as needed
+};
+
+enum ConfigType : uint8_t {
+  TYPE_UINT8 = 0x01,
+  TYPE_UINT16 = 0x02,
+  TYPE_UINT32 = 0x03,
+  TYPE_BOOL = 0x04,
+  TYPE_INT8 = 0x05,
+  TYPE_INT16 = 0x06,
+  TYPE_INT32 = 0x07,
+  TYPE_FLOAT = 0x08,
+  TYPE_STRING = 0x09,
+  TYPE_MOTOR_ARRAY = 0x0A,      // Special type for motor array
+  TYPE_BEHAVIOR_STATES = 0x0B,   // Special type for behavior state array
+  TYPE_VISEME_ARRAY = 0x0C,      // Special type for viseme array
+};
+
 struct FirmwareVersion {
   uint8_t major;
   uint8_t minor;
@@ -17,6 +61,7 @@ struct Motor {
   uint8_t motorID;
   ServoModel servoModel;
   uint16_t position;
+  uint16_t current = 0;
   bool isEnabled = true;
 };
 
@@ -26,16 +71,34 @@ struct RobotConfig {
   std::vector<Motor> motors;
 
   uint16_t getMotorPosition(uint8_t motorID) const;
+  uint16_t getMotorCurrent(uint8_t motorID) const;
   uint8_t getMotorModel(uint8_t motorID);
   bool setMotorPosition(uint8_t motorID, uint16_t newPosition);
+  bool setMotorCurrent(uint8_t motorID, uint16_t newCurrent);
   bool setMotorEnabled(uint8_t motorID, bool enable);
   bool isMotorEnabled(uint8_t motorID);
   void enableAllMotors();
   String serializeJSON() const;
   std::vector<uint8_t> serializeToBytes() const;
 
+  // Legacy format (v1)
   bool saveToFFat(const char* path = "/robot_config.bin") const;
   bool loadFromFFat(const char* path = "/robot_config.bin");
-  bool loadOrCreateDefault(const char* path = "/robot_config.bin");
+  
+  // New key-value format (v2)
+  bool saveToFFatV2(const char* path = "/robot_config.bin", 
+                    BehaviorManager* behaviorManager = nullptr,
+                    VisemeBehavior* visemeBehavior = nullptr) const;
+  bool loadFromFFatV2(const char* path = "/robot_config.bin",
+                      BehaviorManager* behaviorManager = nullptr,
+                      VisemeBehavior* visemeBehavior = nullptr);
+  
+  // New version with behavior/viseme support
+  bool loadOrCreateDefault(const char* path = "/robot_config.bin",
+                           BehaviorManager* behaviorManager = nullptr,
+                           VisemeBehavior* visemeBehavior = nullptr);
+  
+  // Legacy version (for backward compatibility)
+  bool loadOrCreateDefault(const char* path);
 
 };

sensors.cpp

@@ -0,0 +1,340 @@
+#include "sensors.h"
+#include "protocol.h"
+
+// ============================================================================
+// Global Instances
+// ============================================================================
+
+Radar radar;
+ADXL345 adxl;
+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);
+    
+    // Calculate angle from x and y (x is mm from center line, y is distance)
+    // atan2(x, y) gives angle in radians, convert to degrees
+    if (targets[i].valid && targets[i].y > 0) {
+      targets[i].angle = atan2(targets[i].x, targets[i].y) * 180.0f / PI;
+    } else {
+      targets[i].angle = 0.0f;
+    }
+  }
+}
+
+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), angle(2)] * 3 = 28 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);
+    int16_t angle = (int16_t)(targets[i].angle * 10);  // degrees * 10 for precision
+    
+    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;
+    buffer[offset++] = angle & 0xFF;
+    buffer[offset++] = (angle >> 8) & 0xFF;
+  }
+  
+  return offset;
+}
+
+// ============================================================================
+// ADXL345 Implementation
+// ============================================================================
+
+ADXL345::ADXL345(uint8_t addr) : addr(addr) {}
+
+bool ADXL345::init() {
+  Wire.begin(SensorPins::IMU_SDA, SensorPins::IMU_SCL);
+  delay(100);
+
+  uint8_t id = read8(0x00);  // DEVID register (should be 0xE5)
+  if (id != 0xE5) {
+    ready = false;
+    return false;
+  }
+
+  // Enable measurement mode
+  write8(0x2D, 0x08);  // POWER_CTL: measure mode
+
+  ready = true;
+  return true;
+}
+
+bool ADXL345::update() {
+  if (!ready) return false;
+
+  readAccelData();
+  return true;
+}
+
+float ADXL345::getPitch() const {
+  // Approximate pitch from Y/Z acceleration (Y = front/back axis)
+  // This is not as accurate as a proper IMU with gyroscope
+  if (accelZ == 0) return 0;
+  return atan2(-accelY, sqrt(accelX * accelX + accelZ * accelZ)) * 180.0f / PI;
+}
+
+float ADXL345::getRoll() const {
+  // Approximate roll from X/Z acceleration (X = left/right axis)
+  if (accelZ == 0) return 0;
+  return atan2(accelX, accelZ) * 180.0f / PI;
+}
+
+void ADXL345::getEulerAngles(float& pitch, float& roll) const {
+  // Calculate Euler angles from accelerometer data
+  // Note: Heading (yaw) cannot be determined from accelerometer alone
+  // Coordinate system: X=left/right, Y=front/back, Z=up/down
+
+  if (!ready) {
+    pitch = 0.0f;
+    roll = 0.0f;
+    return;
+  }
+
+  // Pitch (front/back tilt) = atan2(-accelY, sqrt(accelX² + accelZ²))
+  // Roll (left/right tilt) = atan2(accelX, accelZ)
+
+  float accelMagnitude = sqrt(accelX * accelX + accelZ * accelZ);
+  if (accelMagnitude > 0.01f) {  // Avoid division by very small numbers
+    pitch = atan2(-accelY, accelMagnitude) * 180.0f / PI;
+  } else {
+    pitch = 0.0f;
+  }
+
+  if (fabs(accelZ) > 0.01f) {  // Avoid division by zero
+    roll = atan2(accelX, accelZ) * 180.0f / PI;
+  } else {
+    roll = 0.0f;
+  }
+}
+
+uint16_t ADXL345::packPayload(uint8_t* buffer) const {
+  // Format: accelX(2) + accelY(2) + accelZ(2) + pitch(2) + roll(2), all ×100
+  // Accelerations in g-forces ×100, angles in degrees ×100
+
+  // Pack acceleration data
+  int16_t x = (int16_t)(accelX * 100.0f);
+  int16_t y = (int16_t)(accelY * 100.0f);
+  int16_t z = (int16_t)(accelZ * 100.0f);
+
+  buffer[0] = x & 0xFF;
+  buffer[1] = (x >> 8) & 0xFF;
+  buffer[2] = y & 0xFF;
+  buffer[3] = (y >> 8) & 0xFF;
+  buffer[4] = z & 0xFF;
+  buffer[5] = (z >> 8) & 0xFF;
+
+  // Calculate and pack Euler angles
+  float pitch_deg, roll_deg;
+  getEulerAngles(pitch_deg, roll_deg);
+
+  int16_t pitch = (int16_t)(pitch_deg * 100.0f);
+  int16_t roll = (int16_t)(roll_deg * 100.0f);
+
+  buffer[6] = pitch & 0xFF;
+  buffer[7] = (pitch >> 8) & 0xFF;
+  buffer[8] = roll & 0xFF;
+  buffer[9] = (roll >> 8) & 0xFF;
+
+  return 10;
+}
+
+void ADXL345::write8(uint8_t reg, uint8_t value) {
+  Wire.beginTransmission(addr);
+  Wire.write(reg);
+  Wire.write(value);
+  Wire.endTransmission();
+}
+
+uint8_t ADXL345::read8(uint8_t reg) {
+  Wire.beginTransmission(addr);
+  Wire.write(reg);
+  Wire.endTransmission();
+  Wire.requestFrom(addr, (uint8_t)1);
+  return Wire.available() ? Wire.read() : 0xFF;
+}
+
+void ADXL345::readAccelData() {
+  // Read 6 bytes starting from DATAX0 register (0x32)
+  Wire.beginTransmission(addr);
+  Wire.write(0x32);  // Start at DATAX0
+  Wire.endTransmission();
+
+  Wire.requestFrom(addr, (uint8_t)6);
+  if (Wire.available() < 6) return;
+
+  // ADXL345 outputs 10-bit values (2 bytes per axis)
+  int16_t x_raw = Wire.read() | (Wire.read() << 8);
+  int16_t y_raw = Wire.read() | (Wire.read() << 8);
+  int16_t z_raw = Wire.read() | (Wire.read() << 8);
+
+  // Convert to g-forces: ±2g range, 10-bit = ±512 LSB, 4mg/LSB
+  const float scale = 0.00390625f;  // 4mg/LSB = 0.004g, but we use 1/256 for 10-bit
+
+  accelX = x_raw * scale;
+  accelY = y_raw * scale;
+  accelZ = z_raw * scale;
+}
+
+// ============================================================================
+// Sensor Manager Implementation
+// ============================================================================
+
+void SensorManager::init() {
+  radar.init();
+
+  if (adxl.init()) {
+    Serial.println("[Sensors] ADXL345 initialized");
+  } else {
+    Serial.println("[Sensors] ADXL345 not detected");
+  }
+
+  Serial.println("[Sensors] Radar initialized");
+}
+
+void SensorManager::update() {
+  // Update sensors
+  radar.update();
+  if (adxl.isReady()) {
+    adxl.update();
+  }
+
+  // Handle streaming
+  unsigned long now = millis();
+
+  if (adxlStreamEnabled && adxl.isReady() && (now - lastADXLSend >= adxlInterval)) {
+    sendADXLPacket();
+    lastADXLSend = now;
+  }
+
+  if (radarStreamEnabled && (now - lastRadarSend >= radarInterval)) {
+    sendRadarPacket();
+    lastRadarSend = now;
+  }
+}
+
+void SensorManager::enableADXLStream(bool enable, uint16_t intervalMs) {
+  adxlStreamEnabled = enable;
+  adxlInterval = intervalMs;
+  lastADXLSend = millis();
+}
+
+void SensorManager::enableRadarStream(bool enable, uint16_t intervalMs) {
+  radarStreamEnabled = enable;
+  radarInterval = intervalMs;
+  lastRadarSend = millis();
+}
+
+void SensorManager::sendADXLPacket() {
+  uint8_t payload[32];  // Buffer sized for current/future payload expansion
+  uint16_t len = adxl.packPayload(payload);
+  sendPacket(Tag::IMU, payload, len);  // Reuse IMU tag for compatibility
+}
+
+void SensorManager::sendRadarPacket() {
+  uint8_t payload[32];  // Updated size for angle field
+  uint16_t len = radar.packPayload(payload);
+  sendPacket(Tag::RADAR, payload, len);
+}
+

sensors.h

@@ -0,0 +1,130 @@
+#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
+  float angle;    // Angle in degrees (calculated from x,y using atan2)
+  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 - ADXL345 3-axis Accelerometer
+// ============================================================================
+
+class ADXL345 {
+public:
+  ADXL345(uint8_t addr = 0x53);
+
+  bool init();
+  bool update();  // Read latest values
+
+  // Get acceleration in g-forces
+  float getAccelX() const { return accelX; }
+  float getAccelY() const { return accelY; }
+  float getAccelZ() const { return accelZ; }
+
+  // Get tilt angles in degrees (approximated from gravity)
+  float getPitch() const;  // Pitch from Y/Z acceleration (Y=front/back)
+  float getRoll() const;   // Roll from X/Z acceleration (X=left/right)
+
+  // Get Euler angles (pitch and roll only, no heading without magnetometer)
+  void getEulerAngles(float& pitch, float& roll) const;
+
+  bool isReady() const { return ready; }
+
+  // Pack into payload buffer (10 bytes: x,y,z accel + pitch,roll angles)
+  uint16_t packPayload(uint8_t* buffer) const;
+
+private:
+  uint8_t addr;
+  bool ready = false;
+  float accelX = 0, accelY = 0, accelZ = 0;
+
+  void write8(uint8_t reg, uint8_t value);
+  uint8_t read8(uint8_t reg);
+  void readAccelData();
+};
+
+// ============================================================================
+// Global Instances
+// ============================================================================
+
+extern Radar radar;
+extern ADXL345 adxl;
+
+// ============================================================================
+// Sensor Manager
+// ============================================================================
+
+class SensorManager {
+public:
+  void init();
+  void update();
+
+  // Streaming control
+  void enableADXLStream(bool enable, uint16_t intervalMs = 100);
+  void enableRadarStream(bool enable, uint16_t intervalMs = 100);
+
+  bool isADXLStreamEnabled() const { return adxlStreamEnabled; }
+  bool isRadarStreamEnabled() const { return radarStreamEnabled; }
+
+private:
+  bool adxlStreamEnabled = true;
+  bool radarStreamEnabled = true;
+  uint16_t adxlInterval = 500;  // 500ms = 2 packets per second
+  uint16_t radarInterval = 10;
+  unsigned long lastADXLSend = 0;
+  unsigned long lastRadarSend = 0;
+
+  void sendADXLPacket();
+  void sendRadarPacket();
+};
+
+extern SensorManager sensors;
+