From 43dc01becefa9a37c2eb38f07d6483b6f94aac70 Mon Sep 17 00:00:00 2001 From: Jake Date: Sun, 25 Jan 2026 14:44:08 +0800 Subject: [PATCH] viseme system implemented --- HansonServo.ino | 35 ++++++-- PROTOCOL_MIGRATION.md | 84 ++++++++++++++++++ behaviors.cpp | 195 ++++++++++++++++++++++++++++++++++++++++++ behaviors.h | 72 ++++++++++++++++ commands.cpp | 136 +++++++++++++++++++++++++++++ commands.h | 7 ++ protocol.h | 7 ++ 7 files changed, 528 insertions(+), 8 deletions(-) diff --git a/HansonServo.ino b/HansonServo.ino index c9b192b..97f8c4d 100644 --- a/HansonServo.ino +++ b/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 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 diff --git a/PROTOCOL_MIGRATION.md b/PROTOCOL_MIGRATION.md index 81e4027..452d65a 100644 --- a/PROTOCOL_MIGRATION.md +++ b/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) diff --git a/behaviors.cpp b/behaviors.cpp index 4e5d8ed..78398ad 100644 --- a/behaviors.cpp +++ b/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& 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(); diff --git a/behaviors.h b/behaviors.h index b13df2d..141c74f 100644 --- a/behaviors.h +++ b/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 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& positions); + + // Get all visemes (for VLST command) + const std::vector& 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 currentPositions; + + // Registered visemes + std::vector 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; diff --git a/commands.cpp b/commands.cpp index f01ecab..4edbd0f 100644 --- a/commands.cpp +++ b/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(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& visemes = visemeBehavior.getVisemes(); + + // Build response payload + std::vector 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 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 // ============================================================================ diff --git a/commands.h b/commands.h index 3c468d2..d88a2e6 100644 --- a/commands.h +++ b/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); diff --git a/protocol.h b/protocol.h index d5d35e0..fca7155 100644 --- a/protocol.h +++ b/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