viseme system implemented

HansonServo.ino

@@ -321,7 +321,7 @@ void runNodeAnimation() {
       if (value != 65535) {
         motorIDs.push_back(motorID);
         positions.push_back(value);
-      } else {
+  } else {
       // Only disable torque for motors that should be limp
         if (config.setMotorEnabled(motorID, false)) {
         servoManager[0]->disableTorque(motorID);
@@ -483,8 +483,8 @@ void setup() {
   // Initialize filesystem
   if (!FFat.begin(true)) {
     Serial.println("[HansonServo] FFat mount failed!");
-    return;
+      return;
-  }
+    }
   Serial.println("[HansonServo] Filesystem ready");
 
   
@@ -496,22 +496,41 @@ void setup() {
     Serial.println("[HansonServo] Config init failed");
   }
 
-  // Initialize behaviors
+  // Initialize behaviors (order determines priority: first added = highest priority)
+  // Priority: Focus > Viseme > Idle
+  
+  // 1. Focus behavior (highest priority - radar tracking)
   static FocusBehavior focusBehavior;
   behaviorManager.addBehavior(BEHAVIOR_FOCUS, &focusBehavior);
   behaviorManager.setBehaviorEnabled(BEHAVIOR_FOCUS, true);
   
-  // Initialize idle behavior with all motor IDs from config
+  // 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;
   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 + idle)");
+  Serial.println("[HansonServo] Behaviors initialized (focus > viseme > idle)");
 
   Serial.println("[HansonServo] Ready");
   Serial.println("[HansonServo] Protocol: 0xA5 0x5A tagged packets with CRC16");
@@ -545,7 +564,7 @@ void loop() {
 // Serial passthrough (when enabled)
 #if ENABLE_SERIAL_PASSTHROUGH
   handleSerialPassthrough();
-  return;
+    return;
 #endif
 
   // Protocol handling

PROTOCOL_MIGRATION.md

@@ -106,6 +106,11 @@ ushort Crc16Ccitt(byte[] data)
 | `RDAR` | Radar target data |
 | `BHVR` | Behavior control (enable/disable) |
 | `BLST` | Behavior list (list all behaviors and states) |
+| `VLST` | List all visemes with names and motor positions |
+| `VADD` | Add a new viseme with name |
+| `VDEL` | Delete a viseme by ID |
+| `VSET` | Set motor positions for a viseme |
+| `VSME` | Trigger viseme (fire-and-forget) |
 | `STAT` | System state/heartbeat |
 | `ACK!` | Acknowledge (success) |
 | `NACK` | Negative acknowledge (failure) |
@@ -287,6 +292,7 @@ For each of 3 targets:
 **Behavior IDs:**
 - `1` = Focus (radar tracking with eye motors 14 & 15)
 - `2` = Idle (perlin noise motion for all motors, ±500 range from center)
+- `3` = Viseme (mouth motor control for speech)
 
 #### `BLST` - Behavior List (host → device)
 **Request:** Empty payload
@@ -302,6 +308,84 @@ For each of 3 targets:
 
 ---
 
+### Visemes
+
+#### `VLST` - List Visemes (host → device)
+**Request:** Empty payload
+
+**Response:**
+```
+[count: 1 byte]                    // Number of visemes
+For each viseme:
+  [viseme_id: 1 byte]
+  [label: 3 bytes]                  // 3-character label (e.g., "AA ", "SIL")
+  [motor_count: 1 byte]
+  For each motor:
+    [motor_id: 1 byte]
+    [position_low: 1 byte]
+    [position_high: 1 byte]
+```
+Position = `position_low | (position_high << 8)`, range 0-4095
+
+#### `VADD` - Add Viseme (host → device)
+**Request:**
+```
+[label: 3 bytes]                    // 3-character label (e.g., "AA ", "SIL")
+```
+
+**Response:** `ACK!` with payload `[new_viseme_id: 1 byte]` on success, `NACK` on failure
+
+#### `VDEL` - Delete Viseme (host → device)
+**Request:**
+```
+[viseme_id: 1 byte]
+```
+
+**Response:** `ACK!` on success, `NACK` if viseme not found
+
+#### `VSET` - Set Viseme Motor Positions (host → device)
+**Request:**
+```
+[viseme_id: 1 byte]
+[motor_count: 1 byte]
+For each motor:
+  [motor_id: 1 byte]
+  [position_low: 1 byte]
+  [position_high: 1 byte]
+```
+
+**Response:** `ACK!` on success, `NACK` if viseme not found
+
+#### `VSME` - Trigger Viseme (host → device)
+**Request:**
+```
+[viseme_id: 1 byte]
+```
+
+**Response:** None (fire-and-forget)
+
+**Notes:**
+- Triggers the viseme behavior which controls the motors defined for that viseme
+- The behavior activates and holds the motor positions
+- After 3 seconds without a new viseme trigger, the behavior deactivates and releases motor control
+- Continuously sending viseme IDs will keep the mouth animated
+- Viseme behavior has higher priority than Idle behavior, lower than Focus
+
+**Default Viseme IDs (loaded at startup):**
+- `0` = Neutral/rest (sil) - motors 40, 43, 44
+- `1` = AA (as in "father")
+- `2` = AE (as in "cat")
+- `3` = AH (as in "but")
+- `4` = AO (as in "bought")
+- `5` = EH (as in "bed")
+- `6` = IH (as in "bit")
+- `7` = IY (as in "beat")
+- `8` = OW (as in "boat")
+- `9` = UH (as in "book")
+- `10` = UW (as in "boot")
+
+---
+
 ### System
 
 #### `STAT` - System State/Heartbeat (device → host)

behaviors.cpp

@@ -270,11 +270,206 @@ bool IdleBehavior::getMotorPosition(uint8_t motorID, uint16_t& position) {
   return false;
 }
 
+// ============================================================================
+// Viseme Behavior Implementation
+// ============================================================================
+
+VisemeBehavior::VisemeBehavior() {
+  isActive = false;
+  lastTriggerTime = 0;
+  nextVisemeID = 0;
+  currentPositions.clear();
+  visemes.clear();
+}
+
+Viseme* VisemeBehavior::findViseme(uint8_t id) {
+  for (Viseme& v : visemes) {
+    if (v.id == id) {
+      return &v;
+    }
+  }
+  return nullptr;
+}
+
+uint8_t VisemeBehavior::addViseme(const char* label) {
+  Viseme newViseme;
+  newViseme.id = nextVisemeID++;
+  
+  // Copy label (3 chars, ensure null-terminated)
+  if (label && strlen(label) >= 3) {
+    newViseme.label[0] = label[0];
+    newViseme.label[1] = label[1];
+    newViseme.label[2] = label[2];
+    newViseme.label[3] = '\0';
+  } else {
+    // Default label if not provided or too short
+    newViseme.label[0] = 'V';
+    newViseme.label[1] = 'I';
+    newViseme.label[2] = 'S';
+    newViseme.label[3] = '\0';
+  }
+  
+  newViseme.motorPositions.clear();
+  visemes.push_back(newViseme);
+  
+  Serial.print("[Viseme] Added viseme '");
+  Serial.print(newViseme.label);
+  Serial.print("' with ID ");
+  Serial.println(newViseme.id);
+  
+  return newViseme.id;
+}
+
+void VisemeBehavior::addViseme(uint8_t id, uint16_t pos40, uint16_t pos43, uint16_t pos44) {
+  // Legacy method for backwards compatibility
+  Viseme* existing = findViseme(id);
+  
+  if (existing) {
+    // Update existing viseme
+    existing->motorPositions.clear();
+    existing->motorPositions.push_back({40, pos40});
+    existing->motorPositions.push_back({43, pos43});
+    existing->motorPositions.push_back({44, pos44});
+  } else {
+    // Add new viseme
+    Viseme newViseme;
+    newViseme.id = id;
+    
+    // Default label based on ID (V + 2 digit ID)
+    newViseme.label[0] = 'V';
+    if (id < 10) {
+      newViseme.label[1] = '0' + id;
+      newViseme.label[2] = ' ';
+    } else if (id < 100) {
+      newViseme.label[1] = '0' + (id / 10);
+      newViseme.label[2] = '0' + (id % 10);
+    } else {
+      newViseme.label[1] = 'X';
+      newViseme.label[2] = 'X';
+    }
+    newViseme.label[3] = '\0';
+    
+    newViseme.motorPositions.push_back({40, pos40});
+    newViseme.motorPositions.push_back({43, pos43});
+    newViseme.motorPositions.push_back({44, pos44});
+    visemes.push_back(newViseme);
+    
+    // Update nextVisemeID if needed
+    if (id >= nextVisemeID) {
+      nextVisemeID = id + 1;
+    }
+  }
+  
+  // Update controlled motors list
+  addMotor(40);
+  addMotor(43);
+  addMotor(44);
+}
+
+bool VisemeBehavior::deleteViseme(uint8_t visemeID) {
+  for (auto it = visemes.begin(); it != visemes.end(); ++it) {
+    if (it->id == visemeID) {
+      Serial.print("[Viseme] Deleted viseme ID ");
+      Serial.println(visemeID);
+      visemes.erase(it);
+      return true;
+    }
+  }
+  
+  Serial.print("[Viseme] Delete failed - unknown viseme ID ");
+  Serial.println(visemeID);
+  return false;
+}
+
+bool VisemeBehavior::setVisemeMotors(uint8_t visemeID, const std::vector<VisemeMotorPosition>& positions) {
+  Viseme* viseme = findViseme(visemeID);
+  if (!viseme) {
+    Serial.print("[Viseme] setVisemeMotors failed - unknown viseme ID ");
+    Serial.println(visemeID);
+    return false;
+  }
+  
+  // Update motor positions
+  viseme->motorPositions = positions;
+  
+  // Update controlled motors list
+  for (const auto& pos : positions) {
+    addMotor(pos.motorID);
+  }
+  
+  Serial.print("[Viseme] Updated viseme ID ");
+  Serial.print(visemeID);
+  Serial.print(" with ");
+  Serial.print(positions.size());
+  Serial.println(" motors");
+  
+  return true;
+}
+
+bool VisemeBehavior::triggerViseme(uint8_t visemeID) {
+  Viseme* viseme = findViseme(visemeID);
+  if (!viseme) {
+    Serial.print("[Viseme] Unknown viseme ID ");
+    Serial.println(visemeID);
+    return false;
+  }
+  
+  // Copy positions for this viseme
+  currentPositions = viseme->motorPositions;
+  
+  // Activate and reset timer
+  isActive = true;
+  lastTriggerTime = millis();
+  
+  Serial.print("[Viseme] Triggered '");
+  Serial.print(viseme->label);
+  Serial.print("' (ID ");
+  Serial.print(visemeID);
+  Serial.println(")");
+  
+  return true;
+}
+
+bool VisemeBehavior::update() {
+  if (!isActive) {
+    return false;
+  }
+  
+  // Check for timeout
+  unsigned long now = millis();
+  if (now - lastTriggerTime >= TIMEOUT_MS) {
+    // Timeout reached - deactivate
+    isActive = false;
+    currentPositions.clear();
+    Serial.println("[Viseme] Timeout - deactivated");
+    return false;
+  }
+  
+  return true;
+}
+
+bool VisemeBehavior::getMotorPosition(uint8_t motorID, uint16_t& position) {
+  if (!isActive) {
+    return false;
+  }
+  
+  // Look up motor in current positions
+  for (const auto& pos : currentPositions) {
+    if (pos.motorID == motorID) {
+      position = pos.position;
+      return true;
+    }
+  }
+  
+  return false;
+}
+
 // ============================================================================
 // Behavior Manager Implementation
 // ============================================================================
 
 BehaviorManager behaviorManager;
+VisemeBehavior visemeBehavior;
 
 BehaviorManager::BehaviorManager() {
   behaviors.clear();

behaviors.h

@@ -12,6 +12,7 @@
 enum BehaviorID : uint8_t {
   BEHAVIOR_FOCUS = 1,  // Focus behavior (radar tracking)
   BEHAVIOR_IDLE = 2,   // Idle behavior (perlin noise for all motors)
+  BEHAVIOR_VISEME = 3, // Viseme behavior (mouth motor positions)
 };
 
 // ============================================================================
@@ -134,6 +135,74 @@ private:
   static constexpr uint16_t MOTOR_SEED_OFFSET = 100; // Seed offset between motors for variety
 };
 
+// ============================================================================
+// Viseme Behavior - Controls mouth motors for speech
+// ============================================================================
+
+// Motor position within a viseme
+struct VisemeMotorPosition {
+  uint8_t motorID;
+  uint16_t position;
+};
+
+// Viseme definition: ID, label (3 chars), and motor positions
+struct Viseme {
+  uint8_t id;
+  char label[4];  // 3 characters + null terminator
+  std::vector<VisemeMotorPosition> motorPositions;
+};
+
+class VisemeBehavior : public Behavior {
+public:
+  VisemeBehavior();
+
+  // Add a viseme with a 3-char label (auto-assigns ID)
+  // Returns the assigned viseme ID
+  uint8_t addViseme(const char* label);
+  
+  // Legacy: Add a viseme with specific ID and motor positions (for backwards compatibility)
+  void addViseme(uint8_t id, uint16_t pos40, uint16_t pos43, uint16_t pos44);
+  
+  // Delete a viseme by ID
+  // Returns true if deleted, false if not found
+  bool deleteViseme(uint8_t visemeID);
+  
+  // Set motor positions for a viseme
+  // Returns true if viseme found and updated, false otherwise
+  bool setVisemeMotors(uint8_t visemeID, const std::vector<VisemeMotorPosition>& positions);
+  
+  // Get all visemes (for VLST command)
+  const std::vector<Viseme>& getVisemes() const { return visemes; }
+  
+  // Trigger a viseme by ID - activates the behavior and sets positions
+  // Returns true if viseme was found, false otherwise
+  bool triggerViseme(uint8_t visemeID);
+
+  // Update behavior - checks for timeout and deactivates
+  bool update() override;
+
+  // Get motor position for a controlled motor
+  bool getMotorPosition(uint8_t motorID, uint16_t& position) override;
+
+private:
+  bool isActive;
+  unsigned long lastTriggerTime;
+  uint8_t nextVisemeID;  // Auto-increment ID for new visemes
+  
+  // Current active motor positions (when triggered)
+  std::vector<VisemeMotorPosition> currentPositions;
+  
+  // Registered visemes
+  std::vector<Viseme> visemes;
+  
+  // Configuration
+  static constexpr unsigned long TIMEOUT_MS = 3000;  // 3 second timeout
+  static constexpr uint16_t DEFAULT_POSITION = 2047; // Center/rest position
+  
+  // Helper to find viseme by ID
+  Viseme* findViseme(uint8_t id);
+};
+
 // ============================================================================
 // Behavior Manager - Manages active behaviors and resolves motor conflicts
 // ============================================================================
@@ -180,3 +249,6 @@ private:
 
 // Global behavior manager instance
 extern BehaviorManager behaviorManager;
+
+// Global viseme behavior instance (for command access)
+extern VisemeBehavior visemeBehavior;

commands.cpp

@@ -136,6 +136,22 @@ void dispatchCommand() {
   else if (tagMatches(tag, Tag::BLST)) {
     handleBehaviorList(payload, len);
   }
+  // Visemes
+  else if (tagMatches(tag, Tag::VLST)) {
+    handleVisemeList(payload, len);
+  }
+  else if (tagMatches(tag, Tag::VADD)) {
+    handleVisemeAdd(payload, len);
+  }
+  else if (tagMatches(tag, Tag::VDEL)) {
+    handleVisemeDelete(payload, len);
+  }
+  else if (tagMatches(tag, Tag::VSET)) {
+    handleVisemeSet(payload, len);
+  }
+  else if (tagMatches(tag, Tag::VSME)) {
+    handleVisemeTrigger(payload, len);
+  }
   // System
   else if (tagMatches(tag, Tag::BOOT)) {
     handleBootloader(payload, len);
@@ -492,6 +508,12 @@ void handleBehaviorList(const uint8_t* payload, uint16_t len) {
         case BEHAVIOR_FOCUS:
           name = "Focus";
           break;
+        case BEHAVIOR_IDLE:
+          name = "Idle";
+          break;
+        case BEHAVIOR_VISEME:
+          name = "Viseme";
+          break;
         default:
           name = "Unknown(" + String(static_cast<uint8_t>(id)) + ")";
           break;
@@ -503,6 +525,120 @@ void handleBehaviorList(const uint8_t* payload, uint16_t len) {
   sendMessage(msg);
 }
 
+// ============================================================================
+// Viseme Handlers
+// ============================================================================
+
+void handleVisemeList(const uint8_t* payload, uint16_t len) {
+  // VLST - returns all visemes with their labels and motor positions
+  const std::vector<Viseme>& visemes = visemeBehavior.getVisemes();
+  
+  // Build response payload
+  std::vector<uint8_t> response;
+  response.push_back(visemes.size());  // count
+  
+  for (const Viseme& v : visemes) {
+    response.push_back(v.id);  // viseme_id
+    
+    // Label (3 bytes)
+    response.push_back(v.label[0]);
+    response.push_back(v.label[1]);
+    response.push_back(v.label[2]);
+    
+    // Motor positions
+    response.push_back(v.motorPositions.size());  // motor_count
+    for (const VisemeMotorPosition& mp : v.motorPositions) {
+      response.push_back(mp.motorID);
+      response.push_back(mp.position & 0xFF);         // position_low
+      response.push_back((mp.position >> 8) & 0xFF);  // position_high
+    }
+  }
+  
+  sendPacket(Tag::VLST, response.data(), response.size());
+}
+
+void handleVisemeAdd(const uint8_t* payload, uint16_t len) {
+  // VADD payload: [label: 3 bytes]
+  if (len < 3) {
+    sendNack(Tag::VADD, "Invalid payload length (need 3-byte label)");
+    return;
+  }
+  
+  // Extract label (3 bytes)
+  char label[4];
+  label[0] = payload[0];
+  label[1] = payload[1];
+  label[2] = payload[2];
+  label[3] = '\0';
+  
+  // Add the viseme
+  uint8_t newID = visemeBehavior.addViseme(label);
+  
+  // Send ACK with the new ID
+  uint8_t ackPayload[1] = { newID };
+  sendPacket(Tag::ACK, ackPayload, 1);
+}
+
+void handleVisemeDelete(const uint8_t* payload, uint16_t len) {
+  // VDEL payload: [viseme_id: 1 byte]
+  if (len < 1) {
+    sendNack(Tag::VDEL, "Invalid payload length");
+    return;
+  }
+  
+  uint8_t visemeID = payload[0];
+  
+  if (visemeBehavior.deleteViseme(visemeID)) {
+    sendAck(Tag::VDEL);
+  } else {
+    sendNack(Tag::VDEL, "Viseme not found");
+  }
+}
+
+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) {
+    sendNack(Tag::VSET, "Invalid payload length");
+    return;
+  }
+  
+  uint8_t visemeID = payload[0];
+  uint8_t motorCount = payload[1];
+  
+  if (len < 2 + motorCount * 3) {
+    sendNack(Tag::VSET, "Motor data truncated");
+    return;
+  }
+  
+  // Parse motor positions
+  std::vector<VisemeMotorPosition> positions;
+  for (uint8_t i = 0; i < motorCount; i++) {
+    uint16_t offset = 2 + i * 3;
+    VisemeMotorPosition mp;
+    mp.motorID = payload[offset];
+    mp.position = payload[offset + 1] | (payload[offset + 2] << 8);
+    positions.push_back(mp);
+  }
+  
+  if (visemeBehavior.setVisemeMotors(visemeID, positions)) {
+    sendAck(Tag::VSET);
+  } else {
+    sendNack(Tag::VSET, "Viseme not found");
+  }
+}
+
+void handleVisemeTrigger(const uint8_t* payload, uint16_t len) {
+  // VSME payload: [viseme_id: 1 byte]
+  // Fire-and-forget - no response expected
+  if (len < 1) {
+    return;  // Silent fail for fire-and-forget
+  }
+  
+  uint8_t visemeID = payload[0];
+  visemeBehavior.triggerViseme(visemeID);
+  // No response sent - fire and forget
+}
+
 // ============================================================================
 // Motor Control Handlers
 // ============================================================================

commands.h

@@ -73,6 +73,13 @@ void handleMotorStream(const uint8_t* payload, uint16_t len);
 void handleBehavior(const uint8_t* payload, uint16_t len);
 void handleBehaviorList(const uint8_t* payload, uint16_t len);
 
+// Visemes
+void handleVisemeList(const uint8_t* payload, uint16_t len);
+void handleVisemeAdd(const uint8_t* payload, uint16_t len);
+void handleVisemeDelete(const uint8_t* payload, uint16_t len);
+void handleVisemeSet(const uint8_t* payload, uint16_t len);
+void handleVisemeTrigger(const uint8_t* payload, uint16_t len);
+
 // System
 void handleBootloader(const uint8_t* payload, uint16_t len);
 

protocol.h

@@ -50,6 +50,13 @@ namespace Tag {
   constexpr char BHVR[4]  = {'B','H','V','R'};  // Behavior control (activate/deactivate)
   constexpr char BLST[4]  = {'B','L','S','T'};  // Behavior list (list all behaviors and their states)
   
+  // Visemes
+  constexpr char VLST[4]  = {'V','L','S','T'};  // List all visemes
+  constexpr char VADD[4]  = {'V','A','D','D'};  // Add a new viseme
+  constexpr char VDEL[4]  = {'V','D','E','L'};  // Delete a viseme
+  constexpr char VSET[4]  = {'V','S','E','T'};  // Set viseme motor positions
+  constexpr char VSME[4]  = {'V','S','M','E'};  // Trigger a viseme by ID
+  
   // System
   constexpr char STATE[4] = {'S','T','A','T'};  // System state/heartbeat
   constexpr char MSGE[4]  = {'M','S','G','E'};  // Log/debug message