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

163
HansonServo.ino
View File

@ -68,33 +68,40 @@ void runNodeAnimation() {
if (!animState.current || !animState.current->isActive()) {
wasActive = false;
return;
}
}
// Reset tick when animation starts
if (!wasActive) {
currentTick = 0;
lastTickTime = millis();
wasActive = true;
}
config.enableAllMotors();
uint32_t now = millis();
if (now - lastTickTime < FRAME_INTERVAL_MS) return;
lastTickTime = now;
lastTickTime = now;
// Tick the node graph
animState.current->nodeGraph.tick(currentTick, *animState.current);
auto outputs = animState.current->nodeGraph.getServoOutputs();
// Collect motor commands
std::vector<uint8_t> motorIDs;
std::vector<uint16_t> positions;
std::vector<uint16_t> speeds;
std::vector<uint8_t> motorIDs;
std::vector<uint16_t> positions;
std::vector<uint16_t> speeds;
for (const auto& [motorID, value] : outputs) {
if (value != 65535) {
motorIDs.push_back(motorID);
positions.push_back(value);
speeds.push_back(0);
config.setMotorPosition(motorID, value);
for (const auto& [motorID, value] : outputs) {
if (value != 65535) {
motorIDs.push_back(motorID);
positions.push_back(value);
speeds.push_back(0);
config.setMotorPosition(motorID, value);
config.setMotorEnabled(motorID, true);
} else {
} else {
// Only disable torque for motors that should be limp
if (config.setMotorEnabled(motorID, false)) {
if (config.setMotorEnabled(motorID, false)) {
servoManager[0]->disableTorque(motorID);
}
}
@ -112,26 +119,27 @@ void runNodeAnimation() {
);
}
currentTick++;
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:
case PLAY_ONCE:
animState.stop();
break;
case PLAY_LOOP:
break;
case PLAY_LOOP:
// Continue looping
break;
case PLAY_REPEAT:
break;
case PLAY_REPEAT:
if (--animState.repeatsRemaining == 0) {
animState.stop();
}
break;
}
break;
default:
break;
}
currentTick = 0;
break;
}
currentTick = 0;
}
}
@ -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
View File

@ -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;
uint16_t filenameLen = payload[0] | (payload[1] << 8);
if (len < 2 + filenameLen + 18) return false;
const uint8_t* ptr = payload;
uint16_t remaining = len;
char filename[128];
memcpy(filename, payload + 2, min((uint16_t)127, filenameLen));
filename[min((uint16_t)127, filenameLen)] = '\0';
// 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;
}
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;
}