diff --git a/HansonServo.ino b/HansonServo.ino index dd36388..dc852b4 100644 --- a/HansonServo.ino +++ b/HansonServo.ino @@ -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 motorIDs; - std::vector positions; - std::vector speeds; + std::vector motorIDs; + std::vector positions; + std::vector 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 } diff --git a/commands.cpp b/commands.cpp index 2596e3a..45cd28c 100644 --- a/commands.cpp +++ b/commands.cpp @@ -4,6 +4,7 @@ #include "esp_system.h" #include "soc/rtc_cntl_reg.h" #include +#include // 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 buffer; + std::vector 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 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; }