updated to send/recieve chunked save files
parent
bc6452c256
commit
7ddb756497
115
HansonServo.ino
115
HansonServo.ino
|
|
@ -70,6 +70,13 @@ void runNodeAnimation() {
|
|||
return;
|
||||
}
|
||||
|
||||
// Reset tick when animation starts
|
||||
if (!wasActive) {
|
||||
currentTick = 0;
|
||||
lastTickTime = millis();
|
||||
wasActive = true;
|
||||
}
|
||||
|
||||
config.enableAllMotors();
|
||||
|
||||
uint32_t now = millis();
|
||||
|
|
@ -114,8 +121,9 @@ void runNodeAnimation() {
|
|||
|
||||
currentTick++;
|
||||
|
||||
// Handle animation end
|
||||
if (currentTick > animState.current->getFrameCount()) {
|
||||
// Handle animation end (0 = run indefinitely for variable-only animations)
|
||||
if (animState.current->getFrameCount() > 0 &&
|
||||
currentTick > animState.current->getFrameCount()) {
|
||||
switch (animState.playMode) {
|
||||
case PLAY_ONCE:
|
||||
animState.stop();
|
||||
|
|
@ -190,79 +198,6 @@ void sendHeartbeat() {
|
|||
sendPacket(Tag::STATE, payload, 6);
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Debug Test Mode
|
||||
// ============================================================================
|
||||
|
||||
// Set to true to run IMU test on startup
|
||||
#define DEBUG_IMU_TEST false
|
||||
|
||||
void runIMUTest() {
|
||||
static unsigned long lastPrint = 0;
|
||||
|
||||
if (millis() - lastPrint < 200) return; // Print every 200ms
|
||||
lastPrint = millis();
|
||||
|
||||
Serial.println("--- IMU TEST ---");
|
||||
|
||||
// Check IMU status
|
||||
Serial.print("IMU Ready: ");
|
||||
Serial.println(imu.isReady() ? "YES" : "NO");
|
||||
|
||||
if (imu.isReady()) {
|
||||
Serial.print(" Heading: ");
|
||||
Serial.print(imu.getHeading(), 1);
|
||||
Serial.print("° Pitch: ");
|
||||
Serial.print(imu.getPitch(), 1);
|
||||
Serial.print("° Roll: ");
|
||||
Serial.print(imu.getRoll(), 1);
|
||||
Serial.println("°");
|
||||
|
||||
// Calculate what the node would output
|
||||
float pitch = constrain(imu.getPitch(), -90.0f, 90.0f);
|
||||
uint16_t pitchValue = (uint16_t)(((pitch + 90.0f) / 180.0f) * 4095.0f);
|
||||
Serial.print(" Pitch as node value: ");
|
||||
Serial.println(pitchValue);
|
||||
}
|
||||
|
||||
// Check if animation is loaded
|
||||
Serial.print("Animation Active: ");
|
||||
Serial.println((animState.current && animState.current->isActive()) ? "YES" : "NO");
|
||||
|
||||
if (animState.current) {
|
||||
Serial.print(" Nodes: ");
|
||||
Serial.print(animState.current->nodeGraph.nodes.size());
|
||||
Serial.print(" Connections: ");
|
||||
Serial.println(animState.current->nodeGraph.connections.size());
|
||||
|
||||
// Print each node's output
|
||||
for (Node* node : animState.current->nodeGraph.nodes) {
|
||||
Serial.print(" Node ");
|
||||
Serial.print(node->id);
|
||||
Serial.print(" (type ");
|
||||
Serial.print(node->type);
|
||||
Serial.print("): in=");
|
||||
Serial.print(node->inputValue);
|
||||
Serial.print(" out=");
|
||||
Serial.println(node->outputValue);
|
||||
}
|
||||
|
||||
// Print servo outputs
|
||||
auto outputs = animState.current->nodeGraph.getServoOutputs();
|
||||
Serial.print(" Servo outputs: ");
|
||||
for (const auto& [motorID, value] : outputs) {
|
||||
Serial.print("M");
|
||||
Serial.print(motorID);
|
||||
Serial.print("=");
|
||||
Serial.print(value);
|
||||
Serial.print(" ");
|
||||
}
|
||||
Serial.println();
|
||||
}
|
||||
|
||||
Serial.println();
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// Setup
|
||||
// ============================================================================
|
||||
|
|
@ -297,31 +232,6 @@ void setup() {
|
|||
Serial.println("[HansonServo] Config init failed");
|
||||
}
|
||||
|
||||
// DEBUG: Load and play IMU test animation
|
||||
#if DEBUG_IMU_TEST
|
||||
Serial.println("[DEBUG] Loading /imutest.anim...");
|
||||
if (animState.animation.loadFromFile("/imutest.anim")) {
|
||||
Serial.println("[DEBUG] Animation loaded successfully!");
|
||||
Serial.print("[DEBUG] Nodes: ");
|
||||
Serial.println(animState.animation.nodeGraph.nodes.size());
|
||||
Serial.print("[DEBUG] Connections: ");
|
||||
Serial.println(animState.animation.nodeGraph.connections.size());
|
||||
|
||||
// Print node details
|
||||
for (Node* node : animState.animation.nodeGraph.nodes) {
|
||||
Serial.print("[DEBUG] Node ");
|
||||
Serial.print(node->id);
|
||||
Serial.print(": type=");
|
||||
Serial.println(node->type);
|
||||
}
|
||||
|
||||
animState.play(PLAY_LOOP);
|
||||
Serial.println("[DEBUG] Animation started in LOOP mode");
|
||||
} else {
|
||||
Serial.println("[DEBUG] Failed to load animation!");
|
||||
}
|
||||
#endif
|
||||
|
||||
Serial.println("[HansonServo] Ready");
|
||||
Serial.println("[HansonServo] Protocol: 0xA5 0x5A tagged packets with CRC16");
|
||||
}
|
||||
|
|
@ -346,9 +256,4 @@ void loop() {
|
|||
|
||||
// Heartbeat
|
||||
sendHeartbeat();
|
||||
|
||||
// DEBUG: Print IMU test info
|
||||
#if DEBUG_IMU_TEST
|
||||
runIMUTest();
|
||||
#endif
|
||||
}
|
||||
|
|
|
|||
194
commands.cpp
194
commands.cpp
|
|
@ -4,6 +4,7 @@
|
|||
#include "esp_system.h"
|
||||
#include "soc/rtc_cntl_reg.h"
|
||||
#include <vector>
|
||||
#include <unordered_map>
|
||||
|
||||
// External references
|
||||
extern RobotConfig config;
|
||||
|
|
@ -13,6 +14,30 @@ extern ServoManager servoManager;
|
|||
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
|
||||
// ============================================================================
|
||||
|
|
@ -210,28 +235,111 @@ void handleFileLoad(const uint8_t* payload, uint16_t len) {
|
|||
}
|
||||
|
||||
size_t fileSize = file.size();
|
||||
if (fileSize > MAX_PAYLOAD_SIZE) {
|
||||
sendNack(Tag::FLOAD, "File too large");
|
||||
file.close();
|
||||
return;
|
||||
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
|
||||
}
|
||||
|
||||
uint8_t* buffer = (uint8_t*)malloc(fileSize);
|
||||
if (!buffer) {
|
||||
sendNack(Tag::FLOAD, "Out of memory");
|
||||
file.close();
|
||||
return;
|
||||
}
|
||||
|
||||
file.read(buffer, fileSize);
|
||||
file.close();
|
||||
|
||||
sendPacket(Tag::FLOAD, buffer, fileSize);
|
||||
free(buffer);
|
||||
}
|
||||
|
||||
void handleFileSave(const uint8_t* payload, uint16_t len) {
|
||||
if (parseAndSaveAnimation(payload, len, animState.animation)) {
|
||||
// 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");
|
||||
|
|
@ -488,41 +596,48 @@ void sendMotorPositions() {
|
|||
}
|
||||
|
||||
bool parseAndSaveAnimation(const uint8_t* payload, uint16_t len, Animation& animation) {
|
||||
if (len < 2) return false;
|
||||
const uint8_t* ptr = payload;
|
||||
uint16_t remaining = len;
|
||||
|
||||
uint16_t filenameLen = payload[0] | (payload[1] << 8);
|
||||
if (len < 2 + filenameLen + 18) return false;
|
||||
// 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;
|
||||
}
|
||||
|
||||
char filename[128];
|
||||
memcpy(filename, payload + 2, min((uint16_t)127, filenameLen));
|
||||
filename[min((uint16_t)127, filenameLen)] = '\0';
|
||||
|
||||
const uint8_t* ptr = payload + 2 + filenameLen;
|
||||
uint16_t remaining = len - 2 - filenameLen;
|
||||
|
||||
sendMessage("Saving: " + String(filename));
|
||||
|
||||
// Parse header
|
||||
if (remaining < 18) return false;
|
||||
// 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;
|
||||
|
||||
uint16_t curveCount = ptr[16] | (ptr[17] << 8);
|
||||
ptr += 18;
|
||||
remaining -= 18;
|
||||
// Curve count (at start of curve block)
|
||||
if (remaining < 2) return false;
|
||||
uint16_t curveCount = ptr[0] | (ptr[1] << 8);
|
||||
ptr += 2;
|
||||
remaining -= 2;
|
||||
|
||||
// Parse curves
|
||||
// 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;
|
||||
|
|
@ -532,17 +647,16 @@ bool parseAndSaveAnimation(const uint8_t* payload, uint16_t len, Animation& anim
|
|||
}
|
||||
remaining -= curveDataSize;
|
||||
|
||||
// Parse node graph
|
||||
// Node graph (whatever remains)
|
||||
if (remaining > 0) {
|
||||
loadNodeGraph(ptr, remaining, animation.nodeGraph);
|
||||
animation.nodeGraph.bindAnimationContext(&animation);
|
||||
}
|
||||
|
||||
// Save to file
|
||||
String fullPath = "/" + String(filename);
|
||||
String fullPath = "/" + filename;
|
||||
animation.saveToFile(fullPath.c_str());
|
||||
|
||||
sendMessage("Saved: " + String(filename));
|
||||
sendMessage("Saved: " + filename);
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue