1169 lines
37 KiB
C++
1169 lines
37 KiB
C++
#include "commands.h"
|
|
#include "nodegraph.h"
|
|
#include "sensors.h"
|
|
#include "behaviors.h"
|
|
#include "websocket_client.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() {
|
|
dispatchCommand(getReceivedTag(), getReceivedPayload(), getReceivedPayloadLen());
|
|
}
|
|
|
|
void dispatchCommand(const char tag[4], const uint8_t* payload, uint16_t len) {
|
|
// 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
|
|
// Supports numeric (uint16) and string values.
|
|
// Dump format: [count:2][id:2][len:2][data:N]...
|
|
// Write format: [id:2][data:N] (len=2 for uint16, len>2 for strings)
|
|
// ============================================================================
|
|
|
|
// Returns true if this setting ID is a string type
|
|
static bool isStringSetting(uint16_t id) {
|
|
return id == SettingID::WIFI_SSID || id == SettingID::WIFI_PASSWORD ||
|
|
id == SettingID::WIFI_HOST || id == SettingID::WIFI_PATH;
|
|
}
|
|
|
|
// Get a numeric setting value by ID. Returns true if valid.
|
|
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;
|
|
// WiFi numeric
|
|
case SettingID::WIFI_PORT: value = wifiSettings.port; break;
|
|
default: return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
// Get a string setting by ID. Returns pointer to string, or nullptr.
|
|
static const char* getSettingString(uint16_t id) {
|
|
switch (id) {
|
|
case SettingID::WIFI_SSID: return wifiSettings.ssid;
|
|
case SettingID::WIFI_PASSWORD: return wifiSettings.password;
|
|
case SettingID::WIFI_HOST: return wifiSettings.host;
|
|
case SettingID::WIFI_PATH: return wifiSettings.path;
|
|
default: return nullptr;
|
|
}
|
|
}
|
|
|
|
// Set a numeric setting value by ID. Returns true if valid.
|
|
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;
|
|
// WiFi numeric
|
|
case SettingID::WIFI_PORT: wifiSettings.port = value; break;
|
|
default: return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
// Set a string setting by ID. Returns true if valid.
|
|
static bool setSettingString(uint16_t id, const char* str, uint16_t strLen) {
|
|
switch (id) {
|
|
case SettingID::WIFI_SSID:
|
|
if (strLen >= sizeof(wifiSettings.ssid)) strLen = sizeof(wifiSettings.ssid) - 1;
|
|
memcpy(wifiSettings.ssid, str, strLen);
|
|
wifiSettings.ssid[strLen] = '\0';
|
|
break;
|
|
case SettingID::WIFI_PASSWORD:
|
|
if (strLen >= sizeof(wifiSettings.password)) strLen = sizeof(wifiSettings.password) - 1;
|
|
memcpy(wifiSettings.password, str, strLen);
|
|
wifiSettings.password[strLen] = '\0';
|
|
break;
|
|
case SettingID::WIFI_HOST:
|
|
if (strLen >= sizeof(wifiSettings.host)) strLen = sizeof(wifiSettings.host) - 1;
|
|
memcpy(wifiSettings.host, str, strLen);
|
|
wifiSettings.host[strLen] = '\0';
|
|
break;
|
|
case SettingID::WIFI_PATH:
|
|
if (strLen >= sizeof(wifiSettings.path)) strLen = sizeof(wifiSettings.path) - 1;
|
|
memcpy(wifiSettings.path, str, strLen);
|
|
wifiSettings.path[strLen] = '\0';
|
|
break;
|
|
default: return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
// Helper: write one setting entry into a buffer at offset. Returns new offset.
|
|
// Format: [id:2][len:2][data:N]
|
|
static uint16_t writeDumpEntry(uint8_t* buf, uint16_t offset, uint16_t id, const uint8_t* data, uint16_t dataLen) {
|
|
buf[offset++] = id & 0xFF;
|
|
buf[offset++] = (id >> 8) & 0xFF;
|
|
buf[offset++] = dataLen & 0xFF;
|
|
buf[offset++] = (dataLen >> 8) & 0xFF;
|
|
memcpy(&buf[offset], data, dataLen);
|
|
offset += dataLen;
|
|
return offset;
|
|
}
|
|
|
|
void handleSettingsSet(const uint8_t* payload, uint16_t len) {
|
|
if (len == 0) {
|
|
// Dump all settings: [count:2][id:2][len:2][data:N]...
|
|
// Max size: 2 + (FOCUS_COUNT + WIFI_COUNT) * (2+2+64) ≈ 1600 bytes, safe for stack
|
|
uint8_t buf[1600];
|
|
uint16_t count = 0;
|
|
uint16_t offset = 2; // Skip count, fill later
|
|
|
|
// Dump all numeric focus settings
|
|
for (uint16_t id = SettingID::FOCUS_FIRST; id <= SettingID::FOCUS_LAST; id++) {
|
|
uint16_t value;
|
|
if (getSettingValue(id, value)) {
|
|
uint8_t vbuf[2] = { (uint8_t)(value & 0xFF), (uint8_t)((value >> 8) & 0xFF) };
|
|
offset = writeDumpEntry(buf, offset, id, vbuf, 2);
|
|
count++;
|
|
}
|
|
}
|
|
|
|
// Dump WiFi string settings
|
|
for (uint16_t id = SettingID::WIFI_FIRST; id <= SettingID::WIFI_LAST; id++) {
|
|
if (isStringSetting(id)) {
|
|
const char* str = getSettingString(id);
|
|
if (str) {
|
|
uint16_t slen = strlen(str);
|
|
offset = writeDumpEntry(buf, offset, id, (const uint8_t*)str, slen);
|
|
count++;
|
|
}
|
|
} else {
|
|
// Numeric WiFi setting (port)
|
|
uint16_t value;
|
|
if (getSettingValue(id, value)) {
|
|
uint8_t vbuf[2] = { (uint8_t)(value & 0xFF), (uint8_t)((value >> 8) & 0xFF) };
|
|
offset = writeDumpEntry(buf, offset, id, vbuf, 2);
|
|
count++;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Write count at the start
|
|
buf[0] = count & 0xFF;
|
|
buf[1] = (count >> 8) & 0xFF;
|
|
|
|
sendPacket(Tag::SSET, buf, offset);
|
|
return;
|
|
}
|
|
|
|
if (len >= 4) {
|
|
// Write a setting: [id:2 LE][data:N]
|
|
uint16_t id = payload[0] | (payload[1] << 8);
|
|
uint16_t dataLen = len - 2;
|
|
const uint8_t* data = &payload[2];
|
|
|
|
bool ok = false;
|
|
bool needsReconnect = false;
|
|
|
|
if (isStringSetting(id)) {
|
|
ok = setSettingString(id, (const char*)data, dataLen);
|
|
needsReconnect = true;
|
|
} else if (dataLen == 2) {
|
|
uint16_t value = data[0] | (data[1] << 8);
|
|
ok = setSettingValue(id, value);
|
|
if (ok && id == SettingID::WIFI_PORT) needsReconnect = true;
|
|
}
|
|
|
|
if (ok) {
|
|
config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior, &focusBehavior);
|
|
sendAck(Tag::SSET);
|
|
if (needsReconnect) websocketReconnect();
|
|
} 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);
|
|
}
|
|
}
|