visemes working and can accept non persistent changes

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,41 @@ 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::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,10 @@ 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);
+  
   // Get all visemes (for VLST command)
   const std::vector<Viseme>& getVisemes() const { return visemes; }
   

commands.cpp

@@ -596,16 +596,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]...
+  // VSET payload: [viseme_id: 1][label: 3 bytes][motor_count: 1][motor_id: 1][pos_low: 1][pos_high: 1]...
-  if (len < 2) {
+  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,14 +622,14 @@ 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)) {
+  if (visemeBehavior.setVisemeMotorsAndLabel(visemeID, label, positions)) {
     sendAck(Tag::VSET);
   } else {
     sendNack(Tag::VSET, "Viseme not found");

ls_firmware.ino

@@ -403,6 +403,12 @@ void runNodeAnimation() {
 // ============================================================================
 
 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)
@@ -505,18 +511,18 @@ void setup() {
   behaviorManager.setBehaviorEnabled(BEHAVIOR_FOCUS, true);
   
   // 2. Viseme behavior (medium priority - mouth animation for speech)
-  // Viseme positions: (id, motor40, motor43, motor44)
+  // Viseme positions: (id, label, motor40, motor43, motor44)
-  visemeBehavior.addViseme(0, 2047, 2047, 2047);  // Neutral/rest (sil)
+  visemeBehavior.addViseme(0, "SIL", 2047, 2047, 2047);  // Neutral/rest (sil)
-  visemeBehavior.addViseme(1, 2200, 1900, 2100);  // AA (as in "father")
+  visemeBehavior.addViseme(1, "AA ", 2200, 1900, 2100);  // AA (as in "father")
-  visemeBehavior.addViseme(2, 2100, 2000, 2000);  // AE (as in "cat")
+  visemeBehavior.addViseme(2, "AE ", 2100, 2000, 2000);  // AE (as in "cat")
-  visemeBehavior.addViseme(3, 2150, 1950, 2050);  // AH (as in "but")
+  visemeBehavior.addViseme(3, "AH ", 2150, 1950, 2050);  // AH (as in "but")
-  visemeBehavior.addViseme(4, 2000, 2100, 1950);  // AO (as in "bought")
+  visemeBehavior.addViseme(4, "AO ", 2000, 2100, 1950);  // AO (as in "bought")
-  visemeBehavior.addViseme(5, 1900, 2200, 1900);  // EH (as in "bed")
+  visemeBehavior.addViseme(5, "EH ", 1900, 2200, 1900);  // EH (as in "bed")
-  visemeBehavior.addViseme(6, 1850, 2250, 1850);  // IH (as in "bit")
+  visemeBehavior.addViseme(6, "IH ", 1850, 2250, 1850);  // IH (as in "bit")
-  visemeBehavior.addViseme(7, 1800, 2300, 1800);  // IY (as in "beat")
+  visemeBehavior.addViseme(7, "IY ", 1800, 2300, 1800);  // IY (as in "beat")
-  visemeBehavior.addViseme(8, 2300, 1800, 2200);  // OW (as in "boat")
+  visemeBehavior.addViseme(8, "OW ", 2300, 1800, 2200);  // OW (as in "boat")
-  visemeBehavior.addViseme(9, 2250, 1850, 2150);  // UH (as in "book")
+  visemeBehavior.addViseme(9, "UH ", 2250, 1850, 2150);  // UH (as in "book")
-  visemeBehavior.addViseme(10, 2200, 1900, 2100); // UW (as in "boot")
+  visemeBehavior.addViseme(10, "UW ", 2200, 1900, 2100); // UW (as in "boot")
   behaviorManager.addBehavior(BEHAVIOR_VISEME, &visemeBehavior);
   behaviorManager.setBehaviorEnabled(BEHAVIOR_VISEME, true);
   
@@ -586,7 +592,7 @@ void loop() {
   handleMotorStreaming();
 
   // Sensor updates and streaming
-  sensors.update();
+  //sensors.update();
 
   // Heartbeat
   sendHeartbeat();