viseme system implemented
parent
2627d26a5b
commit
43dc01bece
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
195
behaviors.cpp
195
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();
|
||||
|
|
|
|||
72
behaviors.h
72
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;
|
||||
|
|
|
|||
136
commands.cpp
136
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
|
||||
// ============================================================================
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in New Issue