1072 lines
34 KiB
C++
1072 lines
34 KiB
C++
#include "commands.h"
|
||
#include "nodegraph.h"
|
||
#include "sensors.h"
|
||
#include "behaviors.h"
|
||
#include "esp_system.h"
|
||
#include "soc/rtc_cntl_reg.h"
|
||
#include <vector>
|
||
#include <unordered_map>
|
||
|
||
// External references
|
||
extern RobotConfig config;
|
||
extern ServoManager servoManager;
|
||
|
||
// Global state instances
|
||
AnimationState animState;
|
||
MotorStreamState motorStream;
|
||
|
||
// ============================================================================
|
||
// Chunked File Save Session (FSAV)
|
||
// ============================================================================
|
||
struct SaveSession {
|
||
bool active = false;
|
||
uint16_t totalChunks = 0;
|
||
uint16_t receivedChunks = 0;
|
||
uint32_t totalSize = 0;
|
||
uint16_t chunkSize = 0; // size of non-final chunks
|
||
std::vector<uint8_t> buffer;
|
||
std::vector<bool> received;
|
||
} g_save;
|
||
|
||
// Helper: reset save session
|
||
static void resetSaveSession() {
|
||
g_save.active = false;
|
||
g_save.totalChunks = 0;
|
||
g_save.receivedChunks = 0;
|
||
g_save.totalSize = 0;
|
||
g_save.chunkSize = 0;
|
||
g_save.buffer.clear();
|
||
g_save.received.clear();
|
||
}
|
||
|
||
// ============================================================================
|
||
// AnimationState
|
||
// ============================================================================
|
||
|
||
void AnimationState::play(PlayMode mode, uint8_t repeats, uint16_t startFrame) {
|
||
current = &animation;
|
||
current->setActive(true);
|
||
playMode = mode;
|
||
repeatsRemaining = repeats;
|
||
this->startFrame = startFrame;
|
||
playGeneration++; // Signal that a new animation has started
|
||
}
|
||
|
||
void AnimationState::stop() {
|
||
if (current) {
|
||
current->setActive(false);
|
||
}
|
||
playMode = PLAY_IDLE;
|
||
}
|
||
|
||
// ============================================================================
|
||
// MotorStreamState
|
||
// ============================================================================
|
||
|
||
void MotorStreamState::start() {
|
||
active = true;
|
||
lastStreamTime = millis();
|
||
}
|
||
|
||
void MotorStreamState::stop() {
|
||
active = false;
|
||
}
|
||
|
||
bool MotorStreamState::shouldStream() {
|
||
if (!active) return false;
|
||
if (millis() - lastStreamTime >= STREAM_INTERVAL_MS) {
|
||
lastStreamTime = millis();
|
||
return true;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
// ============================================================================
|
||
// Command Dispatcher
|
||
// ============================================================================
|
||
|
||
void dispatchCommand() {
|
||
const char* tag = getReceivedTag();
|
||
const uint8_t* payload = getReceivedPayload();
|
||
uint16_t len = getReceivedPayloadLen();
|
||
|
||
// Identity & Config
|
||
if (tagMatches(tag, Tag::IDENT)) {
|
||
handleIdent(payload, len);
|
||
}
|
||
else if (tagMatches(tag, Tag::CONFG)) {
|
||
handleConfig(payload, len);
|
||
}
|
||
// File Operations
|
||
else if (tagMatches(tag, Tag::FLIST)) {
|
||
handleFileList(payload, len);
|
||
}
|
||
else if (tagMatches(tag, Tag::FLOAD)) {
|
||
handleFileLoad(payload, len);
|
||
}
|
||
else if (tagMatches(tag, Tag::FSAVE)) {
|
||
handleFileSave(payload, len);
|
||
}
|
||
else if (tagMatches(tag, Tag::FDELE)) {
|
||
handleFileDelete(payload, len);
|
||
}
|
||
else if (tagMatches(tag, Tag::FPLAY)) {
|
||
handleFilePlay(payload, len);
|
||
}
|
||
else if (tagMatches(tag, Tag::FSTP)) {
|
||
handleFileStop(payload, len);
|
||
}
|
||
// Motor Control
|
||
else if (tagMatches(tag, Tag::MSET)) {
|
||
handleMotorSet(payload, len);
|
||
}
|
||
else if (tagMatches(tag, Tag::MSCAN)) {
|
||
handleMotorScan(payload, len);
|
||
}
|
||
else if (tagMatches(tag, Tag::MWRIT)) {
|
||
handleMotorWrite(payload, len);
|
||
}
|
||
else if (tagMatches(tag, Tag::MSTRM)) {
|
||
handleMotorStream(payload, len);
|
||
}
|
||
// Behaviors
|
||
else if (tagMatches(tag, Tag::BHVR)) {
|
||
handleBehavior(payload, len);
|
||
}
|
||
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);
|
||
}
|
||
// Settings
|
||
else if (tagMatches(tag, Tag::SSET)) {
|
||
handleSettingsSet(payload, len);
|
||
}
|
||
// System
|
||
else if (tagMatches(tag, Tag::BOOT)) {
|
||
handleBootloader(payload, len);
|
||
}
|
||
else {
|
||
char tagStr[5] = {tag[0], tag[1], tag[2], tag[3], 0};
|
||
sendMessage("Unknown tag: " + String(tagStr));
|
||
}
|
||
}
|
||
|
||
// ============================================================================
|
||
// Identity & Config Handlers
|
||
// ============================================================================
|
||
|
||
void handleIdent(const uint8_t* payload, uint16_t len) {
|
||
std::vector<uint8_t> response = config.serializeToBytes();
|
||
sendPacket(Tag::IDENT, response.data(), response.size());
|
||
}
|
||
|
||
void handleConfig(const uint8_t* payload, uint16_t len) {
|
||
RobotConfig newConfig;
|
||
uint16_t offset = 0;
|
||
|
||
// Decode robot name
|
||
if (offset >= len) { sendNack(Tag::CONFG, "Missing name"); return; }
|
||
uint8_t nameLength = payload[offset++];
|
||
for (uint8_t i = 0; i < nameLength && offset < len; ++i) {
|
||
newConfig.deviceName += (char)payload[offset++];
|
||
}
|
||
|
||
// Decode firmware version
|
||
if (offset + 2 > len) { sendNack(Tag::CONFG, "Missing version"); return; }
|
||
newConfig.firmwareVersion.major = payload[offset++];
|
||
newConfig.firmwareVersion.minor = payload[offset++];
|
||
|
||
// Decode motor count
|
||
if (offset >= len) { sendNack(Tag::CONFG, "Missing motors"); return; }
|
||
uint8_t motorCount = payload[offset++];
|
||
|
||
// Decode motors
|
||
for (uint8_t i = 0; i < motorCount && offset < len; ++i) {
|
||
if (offset + 5 > len) break;
|
||
|
||
ServoModel model;
|
||
model.major = payload[offset++];
|
||
model.minor = payload[offset++];
|
||
|
||
uint16_t motorID = payload[offset++] | (payload[offset++] << 8);
|
||
|
||
uint8_t motorNameLength = payload[offset++];
|
||
String motorName = "";
|
||
for (uint8_t j = 0; j < motorNameLength && offset < len; ++j) {
|
||
motorName += (char)payload[offset++];
|
||
}
|
||
|
||
Motor motor;
|
||
motor.motorID = motorID;
|
||
motor.servoModel = model;
|
||
motor.name = motorName;
|
||
motor.position = 0;
|
||
motor.isEnabled = true;
|
||
|
||
newConfig.motors.push_back(motor);
|
||
}
|
||
|
||
config = newConfig;
|
||
config.saveToFFat();
|
||
|
||
sendAck(Tag::CONFG);
|
||
sendMessage("Config updated: " + config.deviceName);
|
||
}
|
||
|
||
// ============================================================================
|
||
// File Operation Handlers
|
||
// ============================================================================
|
||
|
||
void handleFileList(const uint8_t* payload, uint16_t len) {
|
||
File root = FFat.open("/");
|
||
if (!root || !root.isDirectory()) {
|
||
sendPacket(Tag::FLIST, nullptr, 0);
|
||
return;
|
||
}
|
||
|
||
String fileList = "";
|
||
File file = root.openNextFile();
|
||
while (file) {
|
||
if (!file.isDirectory()) {
|
||
fileList += String(file.name()) + "\n";
|
||
}
|
||
file = root.openNextFile();
|
||
}
|
||
|
||
sendPacket(Tag::FLIST, (const uint8_t*)fileList.c_str(), fileList.length());
|
||
}
|
||
|
||
void handleFileLoad(const uint8_t* payload, uint16_t len) {
|
||
if (len == 0 || len >= 128) {
|
||
sendNack(Tag::FLOAD, "Invalid filename");
|
||
return;
|
||
}
|
||
|
||
char filename[128];
|
||
memcpy(filename, payload, len);
|
||
filename[len] = '\0';
|
||
|
||
File file = FFat.open(filename, "r");
|
||
if (!file || !file.available()) {
|
||
sendNack(Tag::FLOAD, "File not found");
|
||
return;
|
||
}
|
||
|
||
size_t fileSize = file.size();
|
||
const uint16_t CHUNK_SIZE = 1024; // match sender chunking
|
||
uint16_t totalChunks = (fileSize + CHUNK_SIZE - 1) / CHUNK_SIZE;
|
||
|
||
// Prepare static header fields
|
||
uint8_t header[8];
|
||
header[0] = totalChunks & 0xFF;
|
||
header[1] = (totalChunks >> 8) & 0xFF;
|
||
header[4] = fileSize & 0xFF;
|
||
header[5] = (fileSize >> 8) & 0xFF;
|
||
header[6] = (fileSize >> 16) & 0xFF;
|
||
header[7] = (fileSize >> 24) & 0xFF;
|
||
|
||
for (uint16_t chunkIndex = 0; chunkIndex < totalChunks; chunkIndex++) {
|
||
size_t offset = (size_t)chunkIndex * CHUNK_SIZE;
|
||
uint16_t thisChunk = (uint16_t)min((size_t)CHUNK_SIZE, fileSize - offset);
|
||
|
||
// Fill per-chunk fields
|
||
header[2] = chunkIndex & 0xFF;
|
||
header[3] = (chunkIndex >> 8) & 0xFF;
|
||
|
||
// Build payload: header + chunkData
|
||
std::vector<uint8_t> payloadBuf;
|
||
payloadBuf.reserve(8 + thisChunk);
|
||
payloadBuf.insert(payloadBuf.end(), header, header + 8);
|
||
|
||
// Read chunk data directly into payload buffer
|
||
size_t startIdx = payloadBuf.size();
|
||
payloadBuf.resize(startIdx + thisChunk);
|
||
file.seek(offset);
|
||
file.read(payloadBuf.data() + startIdx, thisChunk);
|
||
|
||
sendPacket(Tag::FLOAD, payloadBuf.data(), payloadBuf.size());
|
||
delay(2); // small pacing to avoid overwhelming host
|
||
}
|
||
|
||
file.close();
|
||
}
|
||
|
||
void handleFileSave(const uint8_t* payload, uint16_t len) {
|
||
// Chunked protocol:
|
||
// [totalChunks:2][chunkIndex:2][totalSize:4][chunkData:...]
|
||
if (len < 8) {
|
||
sendNack(Tag::FSAVE, "Payload too short");
|
||
return;
|
||
}
|
||
|
||
uint16_t totalChunks = payload[0] | (payload[1] << 8);
|
||
uint16_t chunkIndex = payload[2] | (payload[3] << 8);
|
||
uint32_t totalSize = payload[4] | (payload[5] << 8) | (payload[6] << 16) | (payload[7] << 24);
|
||
const uint8_t* chunkData = payload + 8;
|
||
uint16_t chunkLen = len - 8;
|
||
|
||
// Basic validation
|
||
if (totalChunks == 0) {
|
||
sendNack(Tag::FSAVE, "totalChunks=0");
|
||
return;
|
||
}
|
||
if (chunkIndex >= totalChunks) {
|
||
sendNack(Tag::FSAVE, "chunkIndex out of range");
|
||
return;
|
||
}
|
||
if (totalSize == 0) {
|
||
sendNack(Tag::FSAVE, "totalSize=0");
|
||
return;
|
||
}
|
||
|
||
// Start new session if needed
|
||
if (!g_save.active || g_save.totalSize != totalSize || g_save.totalChunks != totalChunks) {
|
||
g_save.active = true;
|
||
g_save.totalChunks = totalChunks;
|
||
g_save.receivedChunks = 0;
|
||
g_save.totalSize = totalSize;
|
||
g_save.chunkSize = chunkLen; // assume first chunk size is the standard chunk size
|
||
g_save.buffer.assign(totalSize, 0);
|
||
g_save.received.assign(totalChunks, false);
|
||
}
|
||
|
||
// Calculate offset
|
||
uint32_t offset = (uint32_t)chunkIndex * (uint32_t)g_save.chunkSize;
|
||
// For safety, allow last chunk to be smaller
|
||
if (offset + chunkLen > g_save.totalSize) {
|
||
sendNack(Tag::FSAVE, "chunk overflow");
|
||
return;
|
||
}
|
||
|
||
// If not already received, copy in
|
||
if (!g_save.received[chunkIndex]) {
|
||
memcpy(g_save.buffer.data() + offset, chunkData, chunkLen);
|
||
g_save.received[chunkIndex] = true;
|
||
g_save.receivedChunks++;
|
||
}
|
||
|
||
// If not all chunks yet, ACK chunk and return
|
||
if (g_save.receivedChunks < g_save.totalChunks) {
|
||
sendAck(Tag::FSAVE); // per-chunk ACK
|
||
return;
|
||
}
|
||
|
||
// All chunks received - parse and save animation
|
||
bool ok = parseAndSaveAnimation(g_save.buffer.data(), g_save.buffer.size(), animState.animation);
|
||
g_save.active = false;
|
||
g_save.buffer.clear();
|
||
g_save.received.clear();
|
||
|
||
if (ok) {
|
||
sendAck(Tag::FSAVE);
|
||
} else {
|
||
sendNack(Tag::FSAVE, "Parse failed");
|
||
}
|
||
}
|
||
|
||
void handleFileDelete(const uint8_t* payload, uint16_t len) {
|
||
if (len < 2) {
|
||
sendNack(Tag::FDELE, "Invalid request");
|
||
return;
|
||
}
|
||
|
||
uint16_t filenameLen = payload[0] | (payload[1] << 8);
|
||
if (len < 2 + filenameLen) {
|
||
sendNack(Tag::FDELE, "Invalid filename");
|
||
return;
|
||
}
|
||
|
||
char filename[128];
|
||
memcpy(filename, payload + 2, min((uint16_t)127, filenameLen));
|
||
filename[min((uint16_t)127, filenameLen)] = '\0';
|
||
|
||
String fullPath = "/" + String(filename);
|
||
deleteFile(FFat, fullPath.c_str());
|
||
|
||
sendAck(Tag::FDELE);
|
||
sendMessage("Deleted: " + String(filename));
|
||
}
|
||
|
||
void handleFilePlay(const uint8_t* payload, uint16_t len) {
|
||
if (len < 6) { // Minimum: filenameLen(2) + filename(1) + mode(1) + repeats(1) + startFrame(2)
|
||
sendNack(Tag::FPLAY, "Invalid request");
|
||
return;
|
||
}
|
||
|
||
uint16_t filenameLen = payload[0] | (payload[1] << 8);
|
||
if (len < 2 + filenameLen + 4) { // filenameLen + filename + mode + repeats + startFrame
|
||
sendNack(Tag::FPLAY, "Invalid payload length");
|
||
return;
|
||
}
|
||
|
||
char filename[128];
|
||
memcpy(filename, payload + 2, min((uint16_t)127, filenameLen));
|
||
filename[min((uint16_t)127, filenameLen)] = '\0';
|
||
|
||
uint16_t offset = 2 + filenameLen;
|
||
PlayMode mode = static_cast<PlayMode>(payload[offset]);
|
||
uint8_t repeats = payload[offset + 1];
|
||
uint16_t startFrame = payload[offset + 2] | (payload[offset + 3] << 8);
|
||
|
||
// Debug: show parsed startFrame
|
||
sendMessage("FPLAY parsed - startFrame bytes: [" + String(payload[offset + 2]) + ", " + String(payload[offset + 3]) + "] = " + String(startFrame));
|
||
|
||
animState.animation.clear();
|
||
String fullPath = "/" + String(filename);
|
||
|
||
if (animState.animation.loadFromFile(fullPath.c_str())) {
|
||
animState.play(mode, repeats, startFrame);
|
||
sendAck(Tag::FPLAY);
|
||
sendMessage("Playing: " + String(filename) + " from frame " + String(startFrame));
|
||
sendMessage("animState.startFrame stored as: " + String(animState.startFrame));
|
||
} else {
|
||
sendNack(Tag::FPLAY, "Load failed");
|
||
}
|
||
}
|
||
|
||
void handleFileStop(const uint8_t* payload, uint16_t len) {
|
||
// FSTP has no payload (len should be 0, but we don't strictly require it)
|
||
animState.stop();
|
||
sendAck(Tag::FSTP);
|
||
sendMessage("Animation stopped");
|
||
}
|
||
|
||
// ============================================================================
|
||
// Behavior Handler
|
||
// ============================================================================
|
||
|
||
void handleBehavior(const uint8_t* payload, uint16_t len) {
|
||
// BHVR payload: [behaviorID (1 byte)][enable (1 byte)]
|
||
if (len < 2) {
|
||
sendNack(Tag::BHVR, "Invalid payload length");
|
||
return;
|
||
}
|
||
|
||
uint8_t behaviorID = payload[0];
|
||
uint8_t enable = payload[1];
|
||
|
||
// Validate behavior ID
|
||
if (behaviorID == 0 || behaviorID > 255) {
|
||
sendNack(Tag::BHVR, "Invalid behavior ID");
|
||
return;
|
||
}
|
||
|
||
// Enable/disable the behavior
|
||
bool enabled = (enable != 0);
|
||
behaviorManager.setBehaviorEnabled(static_cast<BehaviorID>(behaviorID), enabled);
|
||
|
||
// Save config to persist the behavior state change
|
||
config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior, &focusBehavior);
|
||
|
||
// Send acknowledgment
|
||
sendAck(Tag::BHVR);
|
||
|
||
// Send status message
|
||
String status = enabled ? "enabled" : "disabled";
|
||
sendMessage("Behavior " + String(behaviorID) + " " + status);
|
||
}
|
||
|
||
void handleBehaviorList(const uint8_t* payload, uint16_t len) {
|
||
// BLST payload: none (can be empty)
|
||
// Response: [count (1 byte)][behaviorID1 (1 byte)][enabled1 (1 byte)]...
|
||
|
||
uint8_t count = behaviorManager.getBehaviorCount();
|
||
|
||
// Build response payload: [count][id1][enabled1][id2][enabled2]...
|
||
std::vector<uint8_t> response;
|
||
response.push_back(count);
|
||
|
||
for (uint8_t i = 0; i < count; i++) {
|
||
BehaviorID id;
|
||
bool enabled;
|
||
if (behaviorManager.getBehaviorInfo(i, id, enabled)) {
|
||
response.push_back(static_cast<uint8_t>(id));
|
||
response.push_back(enabled ? 1 : 0);
|
||
}
|
||
}
|
||
|
||
// Send response packet
|
||
sendPacket(Tag::BLST, response.data(), response.size());
|
||
|
||
// Also send debug message with behavior names
|
||
String msg = "Behaviors: ";
|
||
for (uint8_t i = 0; i < count; i++) {
|
||
BehaviorID id;
|
||
bool enabled;
|
||
if (behaviorManager.getBehaviorInfo(i, id, enabled)) {
|
||
if (i > 0) msg += ", ";
|
||
String name;
|
||
switch (id) {
|
||
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;
|
||
}
|
||
msg += name + "(" + String(static_cast<uint8_t>(id)) + ")=";
|
||
msg += enabled ? "ON" : "OFF";
|
||
}
|
||
}
|
||
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);
|
||
|
||
// Save config to persist the new viseme
|
||
config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior, &focusBehavior);
|
||
|
||
// 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)) {
|
||
// Save config to persist the deletion
|
||
config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior, &focusBehavior);
|
||
sendAck(Tag::VDEL);
|
||
} else {
|
||
sendNack(Tag::VDEL, "Viseme not found");
|
||
}
|
||
}
|
||
|
||
void handleVisemeSet(const uint8_t* payload, uint16_t len) {
|
||
// 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];
|
||
|
||
// 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;
|
||
}
|
||
|
||
// Parse motor positions
|
||
std::vector<VisemeMotorPosition> positions;
|
||
for (uint8_t i = 0; i < motorCount; i++) {
|
||
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); // Little-endian
|
||
positions.push_back(mp);
|
||
}
|
||
|
||
// Use createOrUpdateViseme so VSET can create new visemes or update existing ones
|
||
if (visemeBehavior.createOrUpdateViseme(visemeID, label, positions)) {
|
||
// Save config to persist the changes
|
||
config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior, &focusBehavior);
|
||
sendAck(Tag::VSET);
|
||
} else {
|
||
sendNack(Tag::VSET, "Failed to update viseme");
|
||
}
|
||
}
|
||
|
||
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
|
||
// ============================================================================
|
||
|
||
void handleMotorSet(const uint8_t* payload, uint16_t len) {
|
||
if (len % 3 != 0) {
|
||
sendNack(Tag::MSET, "Invalid length");
|
||
return;
|
||
}
|
||
|
||
uint8_t motorCount = len / 3;
|
||
uint8_t ids[motorCount];
|
||
uint16_t positions[motorCount];
|
||
uint16_t speeds[motorCount];
|
||
|
||
for (uint8_t i = 0; i < motorCount; ++i) {
|
||
ids[i] = payload[i * 3];
|
||
positions[i] = (payload[i * 3 + 2] << 8) | payload[i * 3 + 1];
|
||
speeds[i] = 0;
|
||
}
|
||
|
||
servoManager.syncWritePositions(ids, positions, speeds, motorCount, config, 0);
|
||
sendAck(Tag::MSET);
|
||
}
|
||
|
||
void handleMotorScan(const uint8_t* payload, uint16_t len) {
|
||
if (len < 1) {
|
||
sendNack(Tag::MSCAN, "Invalid request");
|
||
return;
|
||
}
|
||
|
||
uint8_t channel = payload[0];
|
||
Feetech* servo = servoManager[channel];
|
||
|
||
for (int i = 0; i < 254; i++) {
|
||
servo->sendPing(i);
|
||
uint8_t val = servo->waitOnData1Byte(10);
|
||
|
||
if (val != 0) {
|
||
uint8_t response[33];
|
||
response[0] = channel;
|
||
response[1] = i;
|
||
|
||
uint16_t model = servo->getModel(i);
|
||
servo->setFeetechMode(model);
|
||
|
||
uint16_t minAngle = servo->getMinAngleLimit(i);
|
||
uint16_t maxAngle = servo->getMaxAngleLimit(i);
|
||
uint16_t position = servo->getPosition(i);
|
||
uint8_t cwDead = servo->getCWDeadZone(i);
|
||
uint8_t ccwDead = servo->getCCWDeadZone(i);
|
||
uint16_t offset = servo->getOffset(i);
|
||
uint8_t mode = servo->getMode(i);
|
||
uint8_t torque = servo->getTorqueEnable(i);
|
||
uint8_t accel = servo->getAcceleration(i);
|
||
uint16_t goalPos = servo->getGoalPosition(i);
|
||
uint16_t goalTime = servo->getGoalTime(i);
|
||
uint16_t goalSpeed = servo->getGoalSpeed(i);
|
||
uint8_t lock = servo->getLock(i);
|
||
int16_t speed = servo->getSpeed(i);
|
||
uint16_t load = servo->getLoad(i);
|
||
uint8_t temp = servo->getTemperature(i);
|
||
uint8_t moving = servo->getMoving(i);
|
||
uint8_t current = servo->getCurrent(i);
|
||
uint8_t voltage = servo->getVoltage(i);
|
||
|
||
// Pack response
|
||
response[2] = (model >> 8) & 0xFF;
|
||
response[3] = model & 0xFF;
|
||
response[4] = (minAngle >> 8) & 0xFF;
|
||
response[5] = minAngle & 0xFF;
|
||
response[6] = (maxAngle >> 8) & 0xFF;
|
||
response[7] = maxAngle & 0xFF;
|
||
response[8] = (position >> 8) & 0xFF;
|
||
response[9] = position & 0xFF;
|
||
response[10] = cwDead;
|
||
response[11] = ccwDead;
|
||
response[12] = (offset >> 8) & 0xFF;
|
||
response[13] = offset & 0xFF;
|
||
response[14] = mode;
|
||
response[15] = torque;
|
||
response[16] = accel;
|
||
response[17] = (goalPos >> 8) & 0xFF;
|
||
response[18] = goalPos & 0xFF;
|
||
response[19] = (goalTime >> 8) & 0xFF;
|
||
response[20] = goalTime & 0xFF;
|
||
response[21] = (goalSpeed >> 8) & 0xFF;
|
||
response[22] = goalSpeed & 0xFF;
|
||
response[23] = lock;
|
||
response[24] = (speed >> 8) & 0xFF;
|
||
response[25] = speed & 0xFF;
|
||
response[26] = (load >> 8) & 0xFF;
|
||
response[27] = load & 0xFF;
|
||
response[28] = temp;
|
||
response[29] = moving;
|
||
response[30] = (current >> 8) & 0xFF;
|
||
response[31] = current & 0xFF;
|
||
response[32] = voltage;
|
||
|
||
sendPacket(Tag::MSCAN, response, 33);
|
||
}
|
||
}
|
||
|
||
// Signal end of scan
|
||
uint8_t endResponse[2] = { channel, 255 };
|
||
sendPacket(Tag::MSCAN, endResponse, 2);
|
||
}
|
||
|
||
void handleMotorWrite(const uint8_t* payload, uint16_t len) {
|
||
if (len < 5) {
|
||
sendNack(Tag::MWRIT, "Invalid request");
|
||
return;
|
||
}
|
||
|
||
uint8_t channel = payload[0];
|
||
uint8_t id = payload[1];
|
||
uint8_t reg = payload[2];
|
||
uint8_t dataLen = payload[3];
|
||
|
||
Feetech* servo = servoManager[channel];
|
||
uint8_t model = servo->getMajorModel(id);
|
||
servo->setFeetechMode(model);
|
||
|
||
// Special case for ID changes (reg 5)
|
||
if (reg == 5 && dataLen == 1) {
|
||
servo->changeID(id, payload[4]);
|
||
sendAck(Tag::MWRIT);
|
||
sendMessage("ID changed");
|
||
return;
|
||
}
|
||
|
||
if (dataLen == 1) {
|
||
servo->write1Byte(id, reg, payload[4]);
|
||
uint8_t response = servo->waitOnData1Byte(10);
|
||
sendPacket(Tag::MWRIT, &response, 1);
|
||
} else {
|
||
servo->write2Bytes(id, reg, payload[4] | (payload[5] << 8));
|
||
uint16_t response = servo->waitOnData2Bytes(10);
|
||
uint8_t respBuf[2] = { (uint8_t)(response & 0xFF), (uint8_t)(response >> 8) };
|
||
sendPacket(Tag::MWRIT, respBuf, 2);
|
||
}
|
||
}
|
||
|
||
void handleMotorStream(const uint8_t* payload, uint16_t len) {
|
||
if (len < 1) {
|
||
sendNack(Tag::MSTRM, "Invalid request");
|
||
return;
|
||
}
|
||
|
||
bool enable = payload[0] != 0;
|
||
|
||
if (enable) {
|
||
// Disable torque for position reading
|
||
for (const Motor& motor : config.motors) {
|
||
servoManager[0]->setFeetechMode(motor.servoModel.major);
|
||
servoManager[0]->disableTorque(motor.motorID);
|
||
}
|
||
motorStream.start();
|
||
} else {
|
||
motorStream.stop();
|
||
}
|
||
|
||
sendAck(Tag::MSTRM);
|
||
}
|
||
|
||
// ============================================================================
|
||
// System Handlers
|
||
// ============================================================================
|
||
// Settings (SSET) - Read/write individual settings by ID
|
||
// ============================================================================
|
||
|
||
// Get a setting value by ID. Returns true if valid ID, fills value.
|
||
static bool getSettingValue(uint16_t id, uint16_t& value) {
|
||
FocusSettings& fs = focusBehavior.getSettings();
|
||
|
||
switch (id) {
|
||
case SettingID::FOCUS_EYE_MOTOR_1: value = fs.eyeMotor1; break;
|
||
case SettingID::FOCUS_EYE_MOTOR_2: value = fs.eyeMotor2; break;
|
||
case SettingID::FOCUS_NECK_MOTOR: value = fs.neckMotor; break;
|
||
case SettingID::FOCUS_EYE_CENTER: value = fs.eyeCenter; break;
|
||
case SettingID::FOCUS_EYE_MIN: value = fs.eyeMin; break;
|
||
case SettingID::FOCUS_EYE_MAX: value = fs.eyeMax; break;
|
||
case SettingID::FOCUS_NECK_CENTER: value = fs.neckCenter; break;
|
||
case SettingID::FOCUS_NECK_MIN: value = fs.neckMin; break;
|
||
case SettingID::FOCUS_NECK_MAX: value = fs.neckMax; break;
|
||
case SettingID::FOCUS_FACE_X_MIN: value = (uint16_t)(int16_t)fs.faceXMin; break;
|
||
case SettingID::FOCUS_FACE_X_MAX: value = (uint16_t)(int16_t)fs.faceXMax; break;
|
||
case SettingID::FOCUS_EYE_SPEED: value = (uint16_t)(fs.eyeSpeed * 1000.0f); break;
|
||
case SettingID::FOCUS_NECK_SPEED: value = (uint16_t)(fs.neckSpeed * 1000.0f); break;
|
||
case SettingID::FOCUS_EYE_RETURN_SPEED: value = (uint16_t)(fs.eyeReturnSpeed * 1000.0f); break;
|
||
case SettingID::FOCUS_NECK_DELAY_MS: value = (uint16_t)fs.neckDelayMs; break;
|
||
case SettingID::FOCUS_NECK_CONTRIBUTION: value = (uint16_t)(fs.neckContribution * 1000.0f); break;
|
||
case SettingID::FOCUS_NECK_INVERT: value = fs.neckInvert ? 1 : 0; break;
|
||
case SettingID::FOCUS_EYE_CENTERING: value = (uint16_t)(fs.eyeCenteringSpeed * 1000.0f); break;
|
||
case SettingID::FOCUS_NECK_CENTERING: value = (uint16_t)(fs.neckCenteringSpeed * 1000.0f); break;
|
||
default: return false;
|
||
}
|
||
return true;
|
||
}
|
||
|
||
// Set a setting value by ID. Returns true if valid ID.
|
||
static bool setSettingValue(uint16_t id, uint16_t value) {
|
||
FocusSettings& fs = focusBehavior.getSettings();
|
||
|
||
switch (id) {
|
||
case SettingID::FOCUS_EYE_MOTOR_1: fs.eyeMotor1 = (uint8_t)value; break;
|
||
case SettingID::FOCUS_EYE_MOTOR_2: fs.eyeMotor2 = (uint8_t)value; break;
|
||
case SettingID::FOCUS_NECK_MOTOR: fs.neckMotor = (uint8_t)value; break;
|
||
case SettingID::FOCUS_EYE_CENTER: fs.eyeCenter = value; break;
|
||
case SettingID::FOCUS_EYE_MIN: fs.eyeMin = value; break;
|
||
case SettingID::FOCUS_EYE_MAX: fs.eyeMax = value; break;
|
||
case SettingID::FOCUS_NECK_CENTER: fs.neckCenter = value; break;
|
||
case SettingID::FOCUS_NECK_MIN: fs.neckMin = value; break;
|
||
case SettingID::FOCUS_NECK_MAX: fs.neckMax = value; break;
|
||
case SettingID::FOCUS_FACE_X_MIN: fs.faceXMin = (float)(int16_t)value; break;
|
||
case SettingID::FOCUS_FACE_X_MAX: fs.faceXMax = (float)(int16_t)value; break;
|
||
case SettingID::FOCUS_EYE_SPEED: fs.eyeSpeed = (float)value / 1000.0f; break;
|
||
case SettingID::FOCUS_NECK_SPEED: fs.neckSpeed = (float)value / 1000.0f; break;
|
||
case SettingID::FOCUS_EYE_RETURN_SPEED: fs.eyeReturnSpeed = (float)value / 1000.0f; break;
|
||
case SettingID::FOCUS_NECK_DELAY_MS: fs.neckDelayMs = (unsigned long)value; break;
|
||
case SettingID::FOCUS_NECK_CONTRIBUTION: fs.neckContribution = (float)value / 1000.0f; break;
|
||
case SettingID::FOCUS_NECK_INVERT: fs.neckInvert = value != 0; break;
|
||
case SettingID::FOCUS_EYE_CENTERING: fs.eyeCenteringSpeed = (float)value / 1000.0f; break;
|
||
case SettingID::FOCUS_NECK_CENTERING: fs.neckCenteringSpeed = (float)value / 1000.0f; break;
|
||
default: return false;
|
||
}
|
||
return true;
|
||
}
|
||
|
||
void handleSettingsSet(const uint8_t* payload, uint16_t len) {
|
||
if (len == 0) {
|
||
// Dump all settings: respond with SSET containing [count:2][id:2][value:2]...
|
||
// Collect all focus settings
|
||
uint8_t buf[2 + SettingID::FOCUS_COUNT * 4]; // count(2) + N × (id(2) + value(2))
|
||
uint16_t count = 0;
|
||
uint16_t offset = 2; // Skip count bytes, fill in later
|
||
|
||
for (uint16_t id = SettingID::FOCUS_FIRST; id <= SettingID::FOCUS_LAST; id++) {
|
||
uint16_t value;
|
||
if (getSettingValue(id, value)) {
|
||
buf[offset++] = id & 0xFF;
|
||
buf[offset++] = (id >> 8) & 0xFF;
|
||
buf[offset++] = value & 0xFF;
|
||
buf[offset++] = (value >> 8) & 0xFF;
|
||
count++;
|
||
}
|
||
}
|
||
|
||
// Write count at the start
|
||
buf[0] = count & 0xFF;
|
||
buf[1] = (count >> 8) & 0xFF;
|
||
|
||
sendPacket(Tag::SSET, buf, offset);
|
||
return;
|
||
}
|
||
|
||
if (len == 4) {
|
||
// Set a single setting: [id:2 LE][value:2 LE]
|
||
uint16_t id = payload[0] | (payload[1] << 8);
|
||
uint16_t value = payload[2] | (payload[3] << 8);
|
||
|
||
if (setSettingValue(id, value)) {
|
||
// Persist to flash
|
||
config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior, &focusBehavior);
|
||
sendAck(Tag::SSET);
|
||
} else {
|
||
sendNack(Tag::SSET, "Unknown setting ID");
|
||
}
|
||
return;
|
||
}
|
||
|
||
sendNack(Tag::SSET, "Invalid payload length");
|
||
}
|
||
|
||
// ============================================================================
|
||
// System
|
||
// ============================================================================
|
||
|
||
void handleBootloader(const uint8_t* payload, uint16_t len) {
|
||
sendMessage("Entering bootloader...");
|
||
Serial.flush();
|
||
delay(100);
|
||
|
||
REG_WRITE(RTC_CNTL_OPTION1_REG, RTC_CNTL_FORCE_DOWNLOAD_BOOT);
|
||
esp_restart();
|
||
}
|
||
|
||
// ============================================================================
|
||
// Helper Functions
|
||
// ============================================================================
|
||
|
||
void sendMotorPositions() {
|
||
std::vector<uint8_t> payload;
|
||
|
||
for (const Motor& motor : config.motors) {
|
||
payload.push_back(motor.motorID);
|
||
payload.push_back(motor.position & 0xFF);
|
||
payload.push_back((motor.position >> 8) & 0xFF);
|
||
}
|
||
|
||
sendPacket(Tag::MPOS, payload.data(), payload.size());
|
||
}
|
||
|
||
bool parseAndSaveAnimation(const uint8_t* payload, uint16_t len, Animation& animation) {
|
||
const uint8_t* ptr = payload;
|
||
uint16_t remaining = len;
|
||
|
||
// Optional filename block (only present if sender included it)
|
||
String filename = "received.anim";
|
||
if (remaining >= 4 && strncmp((const char*)ptr, "ANIM", 4) != 0) {
|
||
if (remaining < 2) return false;
|
||
uint16_t filenameLen = ptr[0] | (ptr[1] << 8);
|
||
ptr += 2;
|
||
remaining -= 2;
|
||
if (filenameLen > 127 || remaining < filenameLen) return false;
|
||
char fname[128];
|
||
memcpy(fname, ptr, filenameLen);
|
||
fname[filenameLen] = '\0';
|
||
filename = fname;
|
||
ptr += filenameLen;
|
||
remaining -= filenameLen;
|
||
}
|
||
|
||
// Header: 16 bytes
|
||
if (remaining < 16) return false;
|
||
memcpy(animation.header.magic, ptr, 4);
|
||
if (strncmp(animation.header.magic, "ANIM", 4) != 0) {
|
||
sendMessage("Invalid magic header");
|
||
return false;
|
||
}
|
||
animation.header.frameCount = ptr[4] | (ptr[5] << 8);
|
||
animation.header.version = ptr[6];
|
||
animation.header.frameRate = ptr[7];
|
||
memcpy(animation.header.reserved, ptr + 8, 8);
|
||
ptr += 16;
|
||
remaining -= 16;
|
||
|
||
if (animation.header.version == 2) {
|
||
// Version 2: Frame data block
|
||
// Calculate motor count from remaining data
|
||
if (animation.header.frameCount == 0) return false;
|
||
uint16_t frameDataSize = remaining;
|
||
uint16_t frameSize = frameDataSize / animation.header.frameCount;
|
||
uint16_t motorCount = frameSize / sizeof(MotorPosition);
|
||
|
||
if (frameSize % sizeof(MotorPosition) != 0) return false;
|
||
if (frameDataSize < animation.header.frameCount * motorCount * sizeof(MotorPosition)) return false;
|
||
|
||
animation.clearFrameData();
|
||
|
||
// Read all frames
|
||
for (uint16_t frameIndex = 0; frameIndex < animation.header.frameCount; frameIndex++) {
|
||
std::vector<MotorPosition> frame;
|
||
frame.reserve(motorCount);
|
||
|
||
for (uint16_t motorIndex = 0; motorIndex < motorCount; motorIndex++) {
|
||
if (remaining < sizeof(MotorPosition)) return false;
|
||
MotorPosition motorPos;
|
||
memcpy(&motorPos, ptr, sizeof(MotorPosition));
|
||
frame.push_back(motorPos);
|
||
ptr += sizeof(MotorPosition);
|
||
remaining -= sizeof(MotorPosition);
|
||
}
|
||
|
||
animation.setFrameData(frameIndex, frame);
|
||
}
|
||
} else {
|
||
// Version 1: Curve count (at start of curve block)
|
||
if (remaining < 2) return false;
|
||
uint16_t curveCount = ptr[0] | (ptr[1] << 8);
|
||
ptr += 2;
|
||
remaining -= 2;
|
||
|
||
// Curves (17 bytes each, packed)
|
||
uint16_t curveDataSize = curveCount * sizeof(CurveSegment);
|
||
if (remaining < curveDataSize) return false;
|
||
animation.clearAllCurves();
|
||
for (uint16_t i = 0; i < curveCount; i++) {
|
||
CurveSegment seg;
|
||
memcpy(&seg, ptr, sizeof(CurveSegment));
|
||
animation.addCurveSegment(seg);
|
||
ptr += sizeof(CurveSegment);
|
||
}
|
||
remaining -= curveDataSize;
|
||
|
||
// Node graph (whatever remains)
|
||
if (remaining > 0) {
|
||
loadNodeGraph(ptr, remaining, animation.nodeGraph);
|
||
animation.nodeGraph.bindAnimationContext(&animation);
|
||
}
|
||
}
|
||
|
||
// Save to file
|
||
String fullPath = "/" + filename;
|
||
animation.saveToFile(fullPath.c_str());
|
||
sendMessage("Saved: " + filename);
|
||
return true;
|
||
}
|
||
|
||
void deleteFile(fs::FS& fs, const char* path) {
|
||
if (fs.remove(path)) {
|
||
Serial.printf("Deleted: %s\n", path);
|
||
} else {
|
||
Serial.printf("Delete failed: %s\n", path);
|
||
}
|
||
}
|