visemes working and can accept non persistent changes
parent
43dc01bece
commit
342a4fe2d2
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -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; }
|
||||
|
||||
|
|
|
|||
23
commands.cpp
23
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<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");
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
Loading…
Reference in New Issue