realrobotshk
/
HansonServo
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

updated to send/recieve chunked save files

protocolv2
Jake 2025-12-10 23:33:21 +08:00
parent bc6452c256
commit 7ddb756497
2 changed files with 188 additions and 169 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

115
HansonServo.ino
View File

@ -70,6 +70,13 @@ void runNodeAnimation() {
return; return;
} }
// Reset tick when animation starts
if (!wasActive) {
currentTick = 0;
lastTickTime = millis();
wasActive = true;
}
config.enableAllMotors(); config.enableAllMotors();
uint32_t now = millis(); uint32_t now = millis();
@ -114,8 +121,9 @@ void runNodeAnimation() {
currentTick++; currentTick++;
// Handle animation end // Handle animation end (0 = run indefinitely for variable-only animations)
if (currentTick > animState.current->getFrameCount()) { if (animState.current->getFrameCount() > 0 &&
currentTick > animState.current->getFrameCount()) {
switch (animState.playMode) { switch (animState.playMode) {
case PLAY_ONCE: case PLAY_ONCE:
animState.stop(); animState.stop();
@ -190,79 +198,6 @@ void sendHeartbeat() {
sendPacket(Tag::STATE, payload, 6); 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 // Setup
// ============================================================================ // ============================================================================
@ -297,31 +232,6 @@ void setup() {
Serial.println("[HansonServo] Config init failed"); 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] Ready");
Serial.println("[HansonServo] Protocol: 0xA5 0x5A tagged packets with CRC16"); Serial.println("[HansonServo] Protocol: 0xA5 0x5A tagged packets with CRC16");
} }
@ -346,9 +256,4 @@ void loop() {
// Heartbeat // Heartbeat
sendHeartbeat(); sendHeartbeat();
// DEBUG: Print IMU test info
#if DEBUG_IMU_TEST
runIMUTest();
#endif
} }

194
commands.cpp
View File

@ -4,6 +4,7 @@
#include "esp_system.h" #include "esp_system.h"
#include "soc/rtc_cntl_reg.h" #include "soc/rtc_cntl_reg.h"
#include <vector> #include <vector>
#include <unordered_map>
// External references // External references
extern RobotConfig config; extern RobotConfig config;
@ -13,6 +14,30 @@ extern ServoManager servoManager;
AnimationState animState; AnimationState animState;
MotorStreamState motorStream; 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 // AnimationState
// ============================================================================ // ============================================================================
@ -210,28 +235,111 @@ void handleFileLoad(const uint8_t* payload, uint16_t len) {
} }
size_t fileSize = file.size(); size_t fileSize = file.size();
if (fileSize > MAX_PAYLOAD_SIZE) { const uint16_t CHUNK_SIZE = 1024; // match sender chunking
sendNack(Tag::FLOAD, "File too large"); uint16_t totalChunks = (fileSize + CHUNK_SIZE - 1) / CHUNK_SIZE;
file.close();
return; // 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(); 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) { 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); sendAck(Tag::FSAVE);
} else { } else {
sendNack(Tag::FSAVE, "Parse failed"); sendNack(Tag::FSAVE, "Parse failed");
@ -488,41 +596,48 @@ void sendMotorPositions() {
} }
bool parseAndSaveAnimation(const uint8_t* payload, uint16_t len, Animation& animation) { 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); // Optional filename block (only present if sender included it)
if (len < 2 + filenameLen + 18) return false; 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]; // Header: 16 bytes
memcpy(filename, payload + 2, min((uint16_t)127, filenameLen)); if (remaining < 16) return false;
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;
memcpy(animation.header.magic, ptr, 4); memcpy(animation.header.magic, ptr, 4);
if (strncmp(animation.header.magic, "ANIM", 4) != 0) { if (strncmp(animation.header.magic, "ANIM", 4) != 0) {
sendMessage("Invalid magic header"); sendMessage("Invalid magic header");
return false; return false;
} }
animation.header.frameCount = ptr[4] | (ptr[5] << 8); animation.header.frameCount = ptr[4] | (ptr[5] << 8);
animation.header.version = ptr[6]; animation.header.version = ptr[6];
animation.header.frameRate = ptr[7]; animation.header.frameRate = ptr[7];
memcpy(animation.header.reserved, ptr + 8, 8); memcpy(animation.header.reserved, ptr + 8, 8);
ptr += 16;
remaining -= 16;
uint16_t curveCount = ptr[16] | (ptr[17] << 8); // Curve count (at start of curve block)
ptr += 18; if (remaining < 2) return false;
remaining -= 18; 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); uint16_t curveDataSize = curveCount * sizeof(CurveSegment);
if (remaining < curveDataSize) return false; if (remaining < curveDataSize) return false;
animation.clearAllCurves(); animation.clearAllCurves();
for (uint16_t i = 0; i < curveCount; i++) { for (uint16_t i = 0; i < curveCount; i++) {
CurveSegment seg; CurveSegment seg;
@ -532,17 +647,16 @@ bool parseAndSaveAnimation(const uint8_t* payload, uint16_t len, Animation& anim
} }
remaining -= curveDataSize; remaining -= curveDataSize;
// Parse node graph // Node graph (whatever remains)
if (remaining > 0) { if (remaining > 0) {
loadNodeGraph(ptr, remaining, animation.nodeGraph); loadNodeGraph(ptr, remaining, animation.nodeGraph);
animation.nodeGraph.bindAnimationContext(&animation); animation.nodeGraph.bindAnimationContext(&animation);
} }
// Save to file // Save to file
String fullPath = "/" + String(filename); String fullPath = "/" + filename;
animation.saveToFile(fullPath.c_str()); animation.saveToFile(fullPath.c_str());
sendMessage("Saved: " + filename);
sendMessage("Saved: " + String(filename));
return true; return true;
} }