motor assist works on feet now. a little over sensitive

behaviors.cpp

@@ -366,6 +366,66 @@ void VisemeBehavior::addViseme(uint8_t id, uint16_t pos40, uint16_t pos43, uint1
   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) {
@@ -406,6 +466,101 @@ bool VisemeBehavior::setVisemeMotors(uint8_t visemeID, const std::vector<VisemeM
   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) {

behaviors.h

@@ -163,6 +163,9 @@ public:
   // 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);
@@ -171,6 +174,14 @@ public:
   // 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; }
   

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");
@@ -596,16 +604,25 @@ void handleVisemeDelete(const uint8_t* payload, uint16_t len) {
 }
 
 void handleVisemeSet(const uint8_t* payload, uint16_t len) {
-  // VSET payload: [viseme_id: 1][motor_count: 1][motor_id: 1][pos_low: 1][pos_high: 1]...
-  if (len < 2) {
+  // 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];
-  uint8_t motorCount = payload[1];
   
-  if (len < 2 + motorCount * 3) {
+  // 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;
   }
@@ -613,17 +630,20 @@ void handleVisemeSet(const uint8_t* payload, uint16_t len) {
   // Parse motor positions
   std::vector<VisemeMotorPosition> positions;
   for (uint8_t i = 0; i < motorCount; i++) {
-    uint16_t offset = 2 + i * 3;
+    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);
+    mp.position = payload[offset + 1] | (payload[offset + 2] << 8);  // Little-endian
     positions.push_back(mp);
   }
   
-  if (visemeBehavior.setVisemeMotors(visemeID, 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");
   }
 }
 

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);
-  //return waitOnData2Bytes(10) * 0.01; FLOAT
-  return waitOnData2Bytes(10);
+  if (feetechMode == MODE_STS || feetechMode == MODE_SMSA || feetechMode == MODE_SMSB) {
+    sendRequest(id, REQUEST_CURRENT_CURRENT, 2);
+    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

@@ -19,6 +19,7 @@
 #include "animation.h"
 #include "commands.h"
 #include "motorcontrol.h"
+#include "motoraid.h"
 #include "nodegraph.h"
 #include "protocol.h"
 #include "sensors.h"
@@ -398,11 +399,57 @@ 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
 // ============================================================================
 
 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)
@@ -413,15 +460,46 @@ void updateMotorPositions() {
     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
 // ============================================================================
@@ -469,6 +547,9 @@ void setup() {
   Serial.setRxBufferSize(8192);
   Serial.begin(1000000);
 
+  pinMode(6, OUTPUT);
+  digitalWrite(6, 1);
+
   // Startup delay
   delay(500);
   Serial.println("\n[HansonServo] Starting...");
@@ -489,48 +570,99 @@ 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, motor40, motor43, motor44)
-  visemeBehavior.addViseme(0, 2047, 2047, 2047);  // Neutral/rest (sil)
-  visemeBehavior.addViseme(1, 2200, 1900, 2100);  // AA (as in "father")
-  visemeBehavior.addViseme(2, 2100, 2000, 2000);  // AE (as in "cat")
-  visemeBehavior.addViseme(3, 2150, 1950, 2050);  // AH (as in "but")
-  visemeBehavior.addViseme(4, 2000, 2100, 1950);  // AO (as in "bought")
-  visemeBehavior.addViseme(5, 1900, 2200, 1900);  // EH (as in "bed")
-  visemeBehavior.addViseme(6, 1850, 2250, 1850);  // IH (as in "bit")
-  visemeBehavior.addViseme(7, 1800, 2300, 1800);  // IY (as in "beat")
-  visemeBehavior.addViseme(8, 2300, 1800, 2200);  // OW (as in "boat")
-  visemeBehavior.addViseme(9, 2250, 1850, 2150);  // UH (as in "book")
-  visemeBehavior.addViseme(10, 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);
+
+  motorStream.start();
   
-  Serial.println("[HansonServo] Behaviors initialized (focus > viseme > idle)");
+  // 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");
@@ -584,10 +716,23 @@ void loop() {
   // Motor position updates
   updateMotorPositions();
   handleMotorStreaming();
-
+  
+  // Motor-aided movement (when motorStream is active)
+  if (motorStream.active) {
+    motorAid.update();
+  }
+  //printMotorStats();
   // Sensor updates and streaming
-  sensors.update();
+  //sensors.update();
 
   // Heartbeat
-  sendHeartbeat();
+  //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;

robotconfig.cpp

@@ -1,4 +1,5 @@
 #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);
 
 };