From 342a4fe2d2a3d9f51433281e52dbf2a25dcfea2e Mon Sep 17 00:00:00 2001 From: Jake Date: Sun, 25 Jan 2026 17:33:08 +0800 Subject: [PATCH] visemes working and can accept non persistent changes --- behaviors.cpp | 95 ++++++++++++++++++++++++++++++ behaviors.h | 7 +++ commands.cpp | 23 +++++--- HansonServo.ino => ls_firmware.ino | 32 ++++++---- 4 files changed, 137 insertions(+), 20 deletions(-) rename HansonServo.ino => ls_firmware.ino (94%) diff --git a/behaviors.cpp b/behaviors.cpp index 78398ad..42c758a 100644 --- a/behaviors.cpp +++ b/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& 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) { diff --git a/behaviors.h b/behaviors.h index 141c74f..263d6a1 100644 --- a/behaviors.h +++ b/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& 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& positions); + // Get all visemes (for VLST command) const std::vector& getVisemes() const { return visemes; } diff --git a/commands.cpp b/commands.cpp index 4edbd0f..e68c0fa 100644 --- a/commands.cpp +++ b/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]... - 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,14 +622,14 @@ void handleVisemeSet(const uint8_t* payload, uint16_t len) { // Parse motor positions std::vector 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"); diff --git a/HansonServo.ino b/ls_firmware.ino similarity index 94% rename from HansonServo.ino rename to ls_firmware.ino index 97f8c4d..51fac23 100644 --- a/HansonServo.ino +++ b/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) - 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") + // Viseme positions: (id, label, motor40, motor43, motor44) + visemeBehavior.addViseme(0, "SIL", 2047, 2047, 2047); // Neutral/rest (sil) + visemeBehavior.addViseme(1, "AA ", 2200, 1900, 2100); // AA (as in "father") + visemeBehavior.addViseme(2, "AE ", 2100, 2000, 2000); // AE (as in "cat") + visemeBehavior.addViseme(3, "AH ", 2150, 1950, 2050); // AH (as in "but") + visemeBehavior.addViseme(4, "AO ", 2000, 2100, 1950); // AO (as in "bought") + visemeBehavior.addViseme(5, "EH ", 1900, 2200, 1900); // EH (as in "bed") + visemeBehavior.addViseme(6, "IH ", 1850, 2250, 1850); // IH (as in "bit") + visemeBehavior.addViseme(7, "IY ", 1800, 2300, 1800); // IY (as in "beat") + visemeBehavior.addViseme(8, "OW ", 2300, 1800, 2200); // OW (as in "boat") + visemeBehavior.addViseme(9, "UH ", 2250, 1850, 2150); // UH (as in "book") + visemeBehavior.addViseme(10, "UW ", 2200, 1900, 2100); // UW (as in "boot") behaviorManager.addBehavior(BEHAVIOR_VISEME, &visemeBehavior); behaviorManager.setBehaviorEnabled(BEHAVIOR_VISEME, true); @@ -586,7 +592,7 @@ void loop() { handleMotorStreaming(); // Sensor updates and streaming - sensors.update(); + //sensors.update(); // Heartbeat sendHeartbeat();