visemes and behaviour state are now stored in NVM

behaviors.cpp

@@ -501,6 +501,66 @@ bool VisemeBehavior::setVisemeMotorsAndLabel(uint8_t visemeID, const char* label
   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) {

behaviors.h

@@ -178,6 +178,10 @@ public:
   // 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; }
   

commands.cpp

@@ -466,6 +466,9 @@ void handleBehavior(const uint8_t* payload, uint16_t len) {
   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);
   
@@ -574,6 +577,9 @@ void handleVisemeAdd(const uint8_t* payload, uint16_t len) {
   // 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);
@@ -589,6 +595,8 @@ void handleVisemeDelete(const uint8_t* payload, uint16_t len) {
   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");
@@ -629,10 +637,13 @@ void handleVisemeSet(const uint8_t* payload, uint16_t len) {
     positions.push_back(mp);
   }
   
-  if (visemeBehavior.setVisemeMotorsAndLabel(visemeID, label, positions)) {
+  // 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, "Viseme not found");
+    sendNack(Tag::VSET, "Failed to update viseme");
   }
 }
 

ls_firmware.ino

@@ -13,7 +13,7 @@
 
 // Configuration defines
 #define ENABLE_SERIAL_PASSTHROUGH                                              \
-  false // Set true to use Feetech app comms straight to servos
+  true // Set true to use Feetech app comms straight to servos
 
 #include "robotconfig.h"
 #include "animation.h"
@@ -398,6 +398,46 @@ void runNodeAnimation() {
   }
 }
 
+// ============================================================================
+// 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
 // ============================================================================
@@ -475,6 +515,9 @@ void setup() {
   Serial.setRxBufferSize(8192);
   Serial.begin(1000000);
 
+  pinMode(6, OUTPUT);
+  digitalWrite(6, 1);
+
   // Startup delay
   delay(500);
   Serial.println("\n[HansonServo] Starting...");
@@ -495,48 +538,80 @@ void setup() {
 
   
 
-  // Load or create robot config
-  if (config.loadOrCreateDefault()) {
-    Serial.println("[HansonServo] Config loaded: " + config.deviceName);
-  } else {
-    Serial.println("[HansonServo] Config init failed");
-  }
-
   // 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);
-  behaviorManager.setBehaviorEnabled(BEHAVIOR_FOCUS, true);
   
   // 2. Viseme behavior (medium priority - mouth animation for speech)
-  // Viseme positions: (id, label, motor40, motor43, motor44)
-  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")
   behaviorManager.addBehavior(BEHAVIOR_VISEME, &visemeBehavior);
-  behaviorManager.setBehaviorEnabled(BEHAVIOR_VISEME, true);
   
   // 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) {
+  for (const Motor& motor : config.motors) {
     allMotorIDs.push_back(motor.motorID);
   }
   idleBehavior.initMotors(allMotorIDs);
-  behaviorManager.addBehavior(BEHAVIOR_IDLE, &idleBehavior);
-  behaviorManager.setBehaviorEnabled(BEHAVIOR_IDLE, true);
-  
-  Serial.println("[HansonServo] Behaviors initialized (focus > viseme > idle)");
 
   Serial.println("[HansonServo] Ready");
   Serial.println("[HansonServo] Protocol: 0xA5 0x5A tagged packets with CRC16");
@@ -596,4 +671,9 @@ void loop() {
 
   // Heartbeat
   sendHeartbeat();
+  
+  // ============================================================================
+  // TEST: Uncomment to enable motor 40 sweep test
+  // ============================================================================
+   //testSweepMotor40();
 }

robotconfig.cpp

@@ -1,4 +1,5 @@
 #include "robotconfig.h"
+#include "behaviors.h"
 #include <FFat.h>
 
 uint16_t RobotConfig::getMotorPosition(uint8_t motorID) const {
@@ -198,10 +199,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 +510,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;
@@ -34,8 +78,24 @@ struct RobotConfig {
   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);
 
 };