SSET settings system implemented, focus settings in NVM and settable with SSET

PROTOCOL_MIGRATION.md

@@ -114,6 +114,7 @@ ushort Crc16Ccitt(byte[] data)
 | `STAT` | System state/heartbeat |
 | `FACE` | Face detection data (Radxa → host/robot, via WiFi) |
 | `ALIV` | Remote component alive/heartbeat (component ID + status) |
+| `SSET` | Settings set/dump (read/write individual settings by ID) |
 | `ACK!` | Acknowledge (success) |
 | `NACK` | Negative acknowledge (failure) |
 | `BOOT` | Enter bootloader |
@@ -421,6 +422,64 @@ For each motor:
 
 ---
 
+### Settings
+
+#### `SSET` - Settings Set/Dump (host ↔ device)
+
+**Write a single setting:**
+**Request:**
+```
+[setting_id: 2 bytes LE]
+[value: 2 bytes LE]
+```
+**Response:** `ACK!` on success, `NACK` if unknown setting ID
+
+**Dump all settings:**
+**Request:** Empty payload (0 bytes)
+**Response:** `SSET` packet:
+```
+[count: 2 bytes LE]
+For each setting:
+  [setting_id: 2 bytes LE]
+  [value: 2 bytes LE]
+```
+
+**Value encoding:**
+- `uint8`/`uint16`/`bool`: stored directly as uint16
+- `float` (0.0–65.535): stored as `value × 1000` (e.g., 0.15 → 150)
+- `int16` (signed): stored as uint16 reinterpret (e.g., -140 → 0xFF74)
+
+**Focus Behavior Setting IDs:**
+
+| ID | Name | Type | Default | Description |
+|----|------|------|---------|-------------|
+| `0x0500` | FOCUS_EYE_MOTOR_1 | uint8 | 14 | Eye motor 1 ID |
+| `0x0501` | FOCUS_EYE_MOTOR_2 | uint8 | 15 | Eye motor 2 ID |
+| `0x0502` | FOCUS_NECK_MOTOR | uint8 | 27 | Neck yaw motor ID |
+| `0x0503` | FOCUS_EYE_CENTER | uint16 | 2200 | Eye servo center position |
+| `0x0504` | FOCUS_EYE_MIN | uint16 | 1700 | Eye servo min position |
+| `0x0505` | FOCUS_EYE_MAX | uint16 | 2500 | Eye servo max position |
+| `0x0506` | FOCUS_NECK_CENTER | uint16 | 2000 | Neck servo center position |
+| `0x0507` | FOCUS_NECK_MIN | uint16 | 1000 | Neck servo min position |
+| `0x0508` | FOCUS_NECK_MAX | uint16 | 3000 | Neck servo max position |
+| `0x0509` | FOCUS_FACE_X_MIN | int16 | -140 | Face detection X min (pixels) |
+| `0x050A` | FOCUS_FACE_X_MAX | int16 | 140 | Face detection X max (pixels) |
+| `0x050B` | FOCUS_EYE_SPEED | float×1000 | 150 | Eye dart speed (0–1) |
+| `0x050C` | FOCUS_NECK_SPEED | float×1000 | 20 | Neck follow speed (0–1) |
+| `0x050D` | FOCUS_EYE_RETURN_SPEED | float×1000 | 50 | Eye return-to-center speed |
+| `0x050E` | FOCUS_NECK_DELAY_MS | uint16 | 500 | Neck start delay (ms) |
+| `0x050F` | FOCUS_NECK_CONTRIBUTION | float×1000 | 700 | Neck offset coverage (0–1) |
+| `0x0510` | FOCUS_NECK_INVERT | bool | 1 | Invert neck motor direction |
+| `0x0511` | FOCUS_EYE_CENTERING | float×1000 | 30 | Eye centering speed (no face) |
+| `0x0512` | FOCUS_NECK_CENTERING | float×1000 | 20 | Neck centering speed (no face) |
+
+**Notes:**
+- Settings are persisted to flash on write
+- The dump response includes all registered settings (currently Focus only, extensible)
+- Future setting ranges (e.g., `0x0600+`) can be added for other behaviors
+
+---
+
 ### System
 
 #### `STAT` - System State/Heartbeat (device → host)

behaviors.cpp

@@ -572,6 +572,7 @@ bool VisemeBehavior::getMotorPosition(uint8_t motorID, uint16_t& position) {
 // ============================================================================
 
 BehaviorManager behaviorManager;
+FocusBehavior focusBehavior;
 VisemeBehavior visemeBehavior;
 
 BehaviorManager::BehaviorManager() {

behaviors.h

@@ -52,6 +52,44 @@ protected:
 // Focus Behavior - Tracks faces with eyes/neck via FaceDetect sensor
 // ============================================================================
 
+// Setting IDs for SSET protocol command
+// Float values are transmitted as fixed-point × 1000 (e.g., 0.15 → 150)
+// Signed values are transmitted as int16 reinterpreted as uint16
+namespace SettingID {
+  // Focus: Motor IDs
+  constexpr uint16_t FOCUS_EYE_MOTOR_1       = 0x0500;
+  constexpr uint16_t FOCUS_EYE_MOTOR_2       = 0x0501;
+  constexpr uint16_t FOCUS_NECK_MOTOR        = 0x0502;
+  // Focus: Eye servo range
+  constexpr uint16_t FOCUS_EYE_CENTER        = 0x0503;
+  constexpr uint16_t FOCUS_EYE_MIN           = 0x0504;
+  constexpr uint16_t FOCUS_EYE_MAX           = 0x0505;
+  // Focus: Neck servo range
+  constexpr uint16_t FOCUS_NECK_CENTER       = 0x0506;
+  constexpr uint16_t FOCUS_NECK_MIN          = 0x0507;
+  constexpr uint16_t FOCUS_NECK_MAX          = 0x0508;
+  // Focus: Face x range (int16, signed)
+  constexpr uint16_t FOCUS_FACE_X_MIN        = 0x0509;
+  constexpr uint16_t FOCUS_FACE_X_MAX        = 0x050A;
+  // Focus: Interpolation speeds (float × 1000)
+  constexpr uint16_t FOCUS_EYE_SPEED         = 0x050B;
+  constexpr uint16_t FOCUS_NECK_SPEED        = 0x050C;
+  constexpr uint16_t FOCUS_EYE_RETURN_SPEED  = 0x050D;
+  // Focus: Neck delay (ms, uint16)
+  constexpr uint16_t FOCUS_NECK_DELAY_MS     = 0x050E;
+  // Focus: Neck contribution (float × 1000)
+  constexpr uint16_t FOCUS_NECK_CONTRIBUTION = 0x050F;
+  // Focus: Neck invert (0 or 1)
+  constexpr uint16_t FOCUS_NECK_INVERT       = 0x0510;
+  // Focus: Centering speeds (float × 1000)
+  constexpr uint16_t FOCUS_EYE_CENTERING     = 0x0511;
+  constexpr uint16_t FOCUS_NECK_CENTERING    = 0x0512;
+  
+  constexpr uint16_t FOCUS_FIRST = 0x0500;
+  constexpr uint16_t FOCUS_LAST  = 0x0512;
+  constexpr uint16_t FOCUS_COUNT = FOCUS_LAST - FOCUS_FIRST + 1;
+}
+
 // Tuneable settings - all values exposed for external adjustment
 struct FocusSettings {
   // Motor IDs
@@ -289,5 +327,6 @@ private:
 // Global behavior manager instance
 extern BehaviorManager behaviorManager;
 
-// Global viseme behavior instance (for command access)
+// Global behavior instances (for command/config access)
+extern FocusBehavior focusBehavior;
 extern VisemeBehavior visemeBehavior;

commands.cpp

@@ -152,6 +152,10 @@ void dispatchCommand() {
   else if (tagMatches(tag, Tag::VSME)) {
     handleVisemeTrigger(payload, len);
   }
+  // Settings
+  else if (tagMatches(tag, Tag::SSET)) {
+    handleSettingsSet(payload, len);
+  }
   // System
   else if (tagMatches(tag, Tag::BOOT)) {
     handleBootloader(payload, len);
@@ -467,7 +471,7 @@ void handleBehavior(const uint8_t* payload, uint16_t len) {
   behaviorManager.setBehaviorEnabled(static_cast<BehaviorID>(behaviorID), enabled);
   
   // Save config to persist the behavior state change
-  config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior);
+  config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior, &focusBehavior);
   
   // Send acknowledgment
   sendAck(Tag::BHVR);
@@ -578,7 +582,7 @@ void handleVisemeAdd(const uint8_t* payload, uint16_t len) {
   uint8_t newID = visemeBehavior.addViseme(label);
   
   // Save config to persist the new viseme
-  config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior);
+  config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior, &focusBehavior);
   
   // Send ACK with the new ID
   uint8_t ackPayload[1] = { newID };
@@ -596,7 +600,7 @@ void handleVisemeDelete(const uint8_t* payload, uint16_t len) {
   
   if (visemeBehavior.deleteViseme(visemeID)) {
     // Save config to persist the deletion
-    config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior);
+    config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior, &focusBehavior);
     sendAck(Tag::VDEL);
   } else {
     sendNack(Tag::VDEL, "Viseme not found");
@@ -640,7 +644,7 @@ void handleVisemeSet(const uint8_t* payload, uint16_t len) {
   // 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);
+    config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior, &focusBehavior);
     sendAck(Tag::VSET);
   } else {
     sendNack(Tag::VSET, "Failed to update viseme");
@@ -827,6 +831,115 @@ void handleMotorStream(const uint8_t* payload, uint16_t len) {
 // ============================================================================
 // System Handlers
 // ============================================================================
+// Settings (SSET) - Read/write individual settings by ID
+// ============================================================================
+
+// Get a setting value by ID. Returns true if valid ID, fills value.
+static bool getSettingValue(uint16_t id, uint16_t& value) {
+  FocusSettings& fs = focusBehavior.getSettings();
+  
+  switch (id) {
+    case SettingID::FOCUS_EYE_MOTOR_1:       value = fs.eyeMotor1; break;
+    case SettingID::FOCUS_EYE_MOTOR_2:       value = fs.eyeMotor2; break;
+    case SettingID::FOCUS_NECK_MOTOR:        value = fs.neckMotor; break;
+    case SettingID::FOCUS_EYE_CENTER:        value = fs.eyeCenter; break;
+    case SettingID::FOCUS_EYE_MIN:           value = fs.eyeMin; break;
+    case SettingID::FOCUS_EYE_MAX:           value = fs.eyeMax; break;
+    case SettingID::FOCUS_NECK_CENTER:       value = fs.neckCenter; break;
+    case SettingID::FOCUS_NECK_MIN:          value = fs.neckMin; break;
+    case SettingID::FOCUS_NECK_MAX:          value = fs.neckMax; break;
+    case SettingID::FOCUS_FACE_X_MIN:        value = (uint16_t)(int16_t)fs.faceXMin; break;
+    case SettingID::FOCUS_FACE_X_MAX:        value = (uint16_t)(int16_t)fs.faceXMax; break;
+    case SettingID::FOCUS_EYE_SPEED:         value = (uint16_t)(fs.eyeSpeed * 1000.0f); break;
+    case SettingID::FOCUS_NECK_SPEED:        value = (uint16_t)(fs.neckSpeed * 1000.0f); break;
+    case SettingID::FOCUS_EYE_RETURN_SPEED:  value = (uint16_t)(fs.eyeReturnSpeed * 1000.0f); break;
+    case SettingID::FOCUS_NECK_DELAY_MS:     value = (uint16_t)fs.neckDelayMs; break;
+    case SettingID::FOCUS_NECK_CONTRIBUTION: value = (uint16_t)(fs.neckContribution * 1000.0f); break;
+    case SettingID::FOCUS_NECK_INVERT:       value = fs.neckInvert ? 1 : 0; break;
+    case SettingID::FOCUS_EYE_CENTERING:     value = (uint16_t)(fs.eyeCenteringSpeed * 1000.0f); break;
+    case SettingID::FOCUS_NECK_CENTERING:    value = (uint16_t)(fs.neckCenteringSpeed * 1000.0f); break;
+    default: return false;
+  }
+  return true;
+}
+
+// Set a setting value by ID. Returns true if valid ID.
+static bool setSettingValue(uint16_t id, uint16_t value) {
+  FocusSettings& fs = focusBehavior.getSettings();
+  
+  switch (id) {
+    case SettingID::FOCUS_EYE_MOTOR_1:       fs.eyeMotor1 = (uint8_t)value; break;
+    case SettingID::FOCUS_EYE_MOTOR_2:       fs.eyeMotor2 = (uint8_t)value; break;
+    case SettingID::FOCUS_NECK_MOTOR:        fs.neckMotor = (uint8_t)value; break;
+    case SettingID::FOCUS_EYE_CENTER:        fs.eyeCenter = value; break;
+    case SettingID::FOCUS_EYE_MIN:           fs.eyeMin = value; break;
+    case SettingID::FOCUS_EYE_MAX:           fs.eyeMax = value; break;
+    case SettingID::FOCUS_NECK_CENTER:       fs.neckCenter = value; break;
+    case SettingID::FOCUS_NECK_MIN:          fs.neckMin = value; break;
+    case SettingID::FOCUS_NECK_MAX:          fs.neckMax = value; break;
+    case SettingID::FOCUS_FACE_X_MIN:        fs.faceXMin = (float)(int16_t)value; break;
+    case SettingID::FOCUS_FACE_X_MAX:        fs.faceXMax = (float)(int16_t)value; break;
+    case SettingID::FOCUS_EYE_SPEED:         fs.eyeSpeed = (float)value / 1000.0f; break;
+    case SettingID::FOCUS_NECK_SPEED:        fs.neckSpeed = (float)value / 1000.0f; break;
+    case SettingID::FOCUS_EYE_RETURN_SPEED:  fs.eyeReturnSpeed = (float)value / 1000.0f; break;
+    case SettingID::FOCUS_NECK_DELAY_MS:     fs.neckDelayMs = (unsigned long)value; break;
+    case SettingID::FOCUS_NECK_CONTRIBUTION: fs.neckContribution = (float)value / 1000.0f; break;
+    case SettingID::FOCUS_NECK_INVERT:       fs.neckInvert = value != 0; break;
+    case SettingID::FOCUS_EYE_CENTERING:     fs.eyeCenteringSpeed = (float)value / 1000.0f; break;
+    case SettingID::FOCUS_NECK_CENTERING:    fs.neckCenteringSpeed = (float)value / 1000.0f; break;
+    default: return false;
+  }
+  return true;
+}
+
+void handleSettingsSet(const uint8_t* payload, uint16_t len) {
+  if (len == 0) {
+    // Dump all settings: respond with SSET containing [count:2][id:2][value:2]...
+    // Collect all focus settings
+    uint8_t buf[2 + SettingID::FOCUS_COUNT * 4];  // count(2) + N × (id(2) + value(2))
+    uint16_t count = 0;
+    uint16_t offset = 2;  // Skip count bytes, fill in later
+    
+    for (uint16_t id = SettingID::FOCUS_FIRST; id <= SettingID::FOCUS_LAST; id++) {
+      uint16_t value;
+      if (getSettingValue(id, value)) {
+        buf[offset++] = id & 0xFF;
+        buf[offset++] = (id >> 8) & 0xFF;
+        buf[offset++] = value & 0xFF;
+        buf[offset++] = (value >> 8) & 0xFF;
+        count++;
+      }
+    }
+    
+    // Write count at the start
+    buf[0] = count & 0xFF;
+    buf[1] = (count >> 8) & 0xFF;
+    
+    sendPacket(Tag::SSET, buf, offset);
+    return;
+  }
+  
+  if (len == 4) {
+    // Set a single setting: [id:2 LE][value:2 LE]
+    uint16_t id = payload[0] | (payload[1] << 8);
+    uint16_t value = payload[2] | (payload[3] << 8);
+    
+    if (setSettingValue(id, value)) {
+      // Persist to flash
+      config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior, &focusBehavior);
+      sendAck(Tag::SSET);
+    } else {
+      sendNack(Tag::SSET, "Unknown setting ID");
+    }
+    return;
+  }
+  
+  sendNack(Tag::SSET, "Invalid payload length");
+}
+
+// ============================================================================
+// System
+// ============================================================================
 
 void handleBootloader(const uint8_t* payload, uint16_t len) {
   sendMessage("Entering bootloader...");

commands.h

@@ -80,6 +80,9 @@ 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);
 
+// Settings
+void handleSettingsSet(const uint8_t* payload, uint16_t len);
+
 // System
 void handleBootloader(const uint8_t* payload, uint16_t len);
 

ls_firmware.ino

@@ -582,8 +582,7 @@ void setup() {
   // 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)
+  // 1. Focus behavior (highest priority - face tracking)
-  static FocusBehavior focusBehavior;
   behaviorManager.addBehavior(BEHAVIOR_FOCUS, &focusBehavior);
   
   // 2. Viseme behavior (medium priority - mouth animation for speech)
@@ -600,7 +599,7 @@ void setup() {
   
   // 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);
+  bool configLoaded = config.loadOrCreateDefault("/robot_config.bin", &behaviorManager, &visemeBehavior, &focusBehavior);
   
   if (configLoaded) {
     Serial.println("[HansonServo] Config loaded: " + config.deviceName);
@@ -613,7 +612,7 @@ void setup() {
       behaviorManager.setBehaviorEnabled(BEHAVIOR_VISEME, true);
       behaviorManager.setBehaviorEnabled(BEHAVIOR_IDLE, true);
       // Save the defaults
-      config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior);
+      config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior, &focusBehavior);
     } else {
       Serial.println("[HansonServo] Behavior states loaded from config");
     }
@@ -634,7 +633,7 @@ void setup() {
       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);
+      config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior, &focusBehavior);
     } else {
       Serial.println("[HansonServo] Visemes loaded from config: " + String(visemeBehavior.getVisemes().size()));
     }

protocol.h

@@ -59,6 +59,9 @@ namespace Tag {
   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
   
+  // Settings
+  constexpr char SSET[4]  = {'S','S','E','T'};  // Settings set/dump (id + value pairs)
+  
   // System
   constexpr char STATE[4] = {'S','T','A','T'};  // System state/heartbeat
   constexpr char MSGE[4]  = {'M','S','G','E'};  // Log/debug message

robotconfig.cpp

@@ -227,7 +227,48 @@ bool RobotConfig::loadOrCreateDefault(const char* path) {
 // Key-Value Format V2 (Compact, Extensible)
 // ============================================================================
 
-bool RobotConfig::saveToFFatV2(const char* path, BehaviorManager* behaviorManager, VisemeBehavior* visemeBehavior) const {
+// ---- Helpers for reading/writing multi-byte values to file ----
+
+static void writeU16(File& f, uint16_t v) {
+  f.write((uint8_t)(v & 0xFF));
+  f.write((uint8_t)((v >> 8) & 0xFF));
+}
+
+static uint16_t readU16(File& f) {
+  uint16_t v = f.read();
+  v |= ((uint16_t)f.read() << 8);
+  return v;
+}
+
+static void writeU32(File& f, uint32_t v) {
+  f.write((uint8_t)(v & 0xFF));
+  f.write((uint8_t)((v >> 8) & 0xFF));
+  f.write((uint8_t)((v >> 16) & 0xFF));
+  f.write((uint8_t)((v >> 24) & 0xFF));
+}
+
+static uint32_t readU32(File& f) {
+  uint32_t v = f.read();
+  v |= ((uint32_t)f.read() << 8);
+  v |= ((uint32_t)f.read() << 16);
+  v |= ((uint32_t)f.read() << 24);
+  return v;
+}
+
+static void writeFloat(File& f, float v) {
+  uint32_t raw;
+  memcpy(&raw, &v, 4);
+  writeU32(f, raw);
+}
+
+static float readFloat(File& f) {
+  uint32_t raw = readU32(f);
+  float v;
+  memcpy(&v, &raw, 4);
+  return v;
+}
+
+bool RobotConfig::saveToFFatV2(const char* path, BehaviorManager* behaviorManager, VisemeBehavior* visemeBehavior, FocusBehavior* focusBehavior) const {
   File file = FFat.open(path, FILE_WRITE);
   if (!file) return false;
 
@@ -343,6 +384,54 @@ bool RobotConfig::saveToFFatV2(const char* path, BehaviorManager* behaviorManage
     }
   }
 
+  // Setting 7: Focus Behavior Settings
+  if (focusBehavior) {
+    const FocusSettings& fs = focusBehavior->getSettings();
+    
+    file.write((uint8_t)(KEY_FOCUS_SETTINGS & 0xFF));
+    file.write((uint8_t)((KEY_FOCUS_SETTINGS >> 8) & 0xFF));
+    file.write(TYPE_FOCUS_SETTINGS);
+    
+    // Motor IDs
+    file.write(fs.eyeMotor1);
+    file.write(fs.eyeMotor2);
+    file.write(fs.neckMotor);
+    
+    // Eye servo range
+    writeU16(file, fs.eyeCenter);
+    writeU16(file, fs.eyeMin);
+    writeU16(file, fs.eyeMax);
+    
+    // Neck servo range
+    writeU16(file, fs.neckCenter);
+    writeU16(file, fs.neckMin);
+    writeU16(file, fs.neckMax);
+    
+    // Face x range
+    writeFloat(file, fs.faceXMin);
+    writeFloat(file, fs.faceXMax);
+    
+    // Interpolation speeds
+    writeFloat(file, fs.eyeSpeed);
+    writeFloat(file, fs.neckSpeed);
+    writeFloat(file, fs.eyeReturnSpeed);
+    
+    // Neck delay
+    writeU32(file, (uint32_t)fs.neckDelayMs);
+    
+    // Neck contribution
+    writeFloat(file, fs.neckContribution);
+    
+    // Neck invert
+    file.write(fs.neckInvert ? 1 : 0);
+    
+    // Centering speeds
+    writeFloat(file, fs.eyeCenteringSpeed);
+    writeFloat(file, fs.neckCenteringSpeed);
+    
+    settingCount++;
+  }
+
   // Write setting count at the beginning
   size_t endPos = file.position();
   file.seek(countPos);
@@ -354,7 +443,7 @@ bool RobotConfig::saveToFFatV2(const char* path, BehaviorManager* behaviorManage
   return true;
 }
 
-bool RobotConfig::loadFromFFatV2(const char* path, BehaviorManager* behaviorManager, VisemeBehavior* visemeBehavior) {
+bool RobotConfig::loadFromFFatV2(const char* path, BehaviorManager* behaviorManager, VisemeBehavior* visemeBehavior, FocusBehavior* focusBehavior) {
   File file = FFat.open(path, FILE_READ);
   if (!file) return false;
 
@@ -479,6 +568,55 @@ bool RobotConfig::loadFromFFatV2(const char* path, BehaviorManager* behaviorMana
         break;
       }
 
+      case KEY_FOCUS_SETTINGS: {
+        if (type == TYPE_FOCUS_SETTINGS && focusBehavior) {
+          FocusSettings& fs = focusBehavior->getSettings();
+          
+          // Motor IDs
+          fs.eyeMotor1 = file.read();
+          fs.eyeMotor2 = file.read();
+          fs.neckMotor = file.read();
+          
+          // Eye servo range
+          fs.eyeCenter = readU16(file);
+          fs.eyeMin = readU16(file);
+          fs.eyeMax = readU16(file);
+          
+          // Neck servo range
+          fs.neckCenter = readU16(file);
+          fs.neckMin = readU16(file);
+          fs.neckMax = readU16(file);
+          
+          // Face x range
+          fs.faceXMin = readFloat(file);
+          fs.faceXMax = readFloat(file);
+          
+          // Interpolation speeds
+          fs.eyeSpeed = readFloat(file);
+          fs.neckSpeed = readFloat(file);
+          fs.eyeReturnSpeed = readFloat(file);
+          
+          // Neck delay
+          fs.neckDelayMs = (unsigned long)readU32(file);
+          
+          // Neck contribution
+          fs.neckContribution = readFloat(file);
+          
+          // Neck invert
+          fs.neckInvert = file.read() != 0;
+          
+          // Centering speeds
+          fs.eyeCenteringSpeed = readFloat(file);
+          fs.neckCenteringSpeed = readFloat(file);
+          
+          Serial.println("[Config] Focus settings loaded");
+        } else {
+          // Skip the focus settings blob (52 bytes) if no focusBehavior
+          for (int k = 0; k < 52; k++) file.read();
+        }
+        break;
+      }
+
       default:
         // Unknown key - skip based on type
         switch (type) {
@@ -507,11 +645,11 @@ bool RobotConfig::loadFromFFatV2(const char* path, BehaviorManager* behaviorMana
   return true;
 }
 
-bool RobotConfig::loadOrCreateDefault(const char* path, BehaviorManager* behaviorManager, VisemeBehavior* visemeBehavior) {
+bool RobotConfig::loadOrCreateDefault(const char* path, BehaviorManager* behaviorManager, VisemeBehavior* visemeBehavior, FocusBehavior* focusBehavior) {
   if (FFat.exists(path)) {
     Serial.println("Loading robot config from FFat...");
     // Try V2 format first
-    if (loadFromFFatV2(path, behaviorManager, visemeBehavior)) {
+    if (loadFromFFatV2(path, behaviorManager, visemeBehavior, focusBehavior)) {
       Serial.println("Loaded V2 format");
       return true;
     }
@@ -519,17 +657,17 @@ bool RobotConfig::loadOrCreateDefault(const char* path, BehaviorManager* behavio
     if (loadFromFFat(path)) {
       Serial.println("Loaded V1 format (legacy)");
       // Upgrade to V2 format
-      saveToFFatV2(path, behaviorManager, visemeBehavior);
+      saveToFFatV2(path, behaviorManager, visemeBehavior, focusBehavior);
       return true;
     }
   }
 
   Serial.println("No config found. Creating default config...");
 
-  // 🔧 Define your default config here
+  // Define your default config here
   deviceName = "DefaultBot";
   firmwareVersion = { 1, 0 };
   motors.clear();
 
-  return saveToFFatV2(path, behaviorManager, visemeBehavior);
+  return saveToFFatV2(path, behaviorManager, visemeBehavior, focusBehavior);
 }

robotconfig.h

@@ -5,6 +5,7 @@
 // Forward declarations
 class BehaviorManager;
 class VisemeBehavior;
+class FocusBehavior;
 
 // ============================================================================
 // Config Key-Value System
@@ -25,6 +26,9 @@ enum ConfigKey : uint16_t {
   // Viseme array (single entry containing all visemes)
   KEY_VISEME_ARRAY = 0x0300,
   
+  // Focus behavior settings
+  KEY_FOCUS_SETTINGS = 0x0500,
+  
   // Future extensible settings
   KEY_SERIAL_BAUD = 0x0400,
   KEY_MOTOR_UPDATE_INTERVAL = 0x0401,
@@ -44,6 +48,7 @@ enum ConfigType : uint8_t {
   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
+  TYPE_FOCUS_SETTINGS = 0x0D,    // Focus behavior settings blob
 };
 
 struct FirmwareVersion {
@@ -88,15 +93,18 @@ struct RobotConfig {
   // New key-value format (v2)
   bool saveToFFatV2(const char* path = "/robot_config.bin", 
                     BehaviorManager* behaviorManager = nullptr,
-                    VisemeBehavior* visemeBehavior = nullptr) const;
+                    VisemeBehavior* visemeBehavior = nullptr,
+                    FocusBehavior* focusBehavior = nullptr) const;
   bool loadFromFFatV2(const char* path = "/robot_config.bin",
                       BehaviorManager* behaviorManager = nullptr,
-                      VisemeBehavior* visemeBehavior = nullptr);
+                      VisemeBehavior* visemeBehavior = nullptr,
+                      FocusBehavior* focusBehavior = nullptr);
   
   // New version with behavior/viseme support
   bool loadOrCreateDefault(const char* path = "/robot_config.bin",
                            BehaviorManager* behaviorManager = nullptr,
-                           VisemeBehavior* visemeBehavior = nullptr);
+                           VisemeBehavior* visemeBehavior = nullptr,
+                           FocusBehavior* focusBehavior = nullptr);
   
   // Legacy version (for backward compatibility)
   bool loadOrCreateDefault(const char* path);