From 2a1b4bd2766e39ec2ab6efd626f70c209aed118f Mon Sep 17 00:00:00 2001 From: Jake Date: Tue, 13 Jan 2026 23:27:38 +0800 Subject: [PATCH] animation file v2 implemented, might break old connections. playback at fps defined by animation file --- ANIM_V2 | 62 +++++++++++++++ HansonServo.ino | 179 ++++++++++++++++++++++++++++++++++++++++--- animation.cpp | 200 ++++++++++++++++++++++++++++++++++++++---------- animation.h | 16 +++- commands.cpp | 72 ++++++++++++----- commands.h | 1 + 6 files changed, 458 insertions(+), 72 deletions(-) create mode 100644 ANIM_V2 diff --git a/ANIM_V2 b/ANIM_V2 new file mode 100644 index 0000000..46674c9 --- /dev/null +++ b/ANIM_V2 @@ -0,0 +1,62 @@ +# Animation File Format Version 2 - Encoding Specification + +## File Structure + +The file consists of three sections in order: + +1. **Filename Block** (optional, for serial transmission) +2. **Header Block** (16 bytes) +3. **Frame Data Block** (variable size) + +## Encoding Details + +### 1. Filename Block +``` +[filename_length: 2 bytes, uint16_t, little-endian] +[filename_bytes: N bytes, UTF-8 string] +``` + +### 2. Header Block (16 bytes total) +``` +[0-3] "ANIM" (4 bytes, ASCII) +[4-5] frameCount (2 bytes, uint16_t, little-endian) +[6] version (1 byte, uint8_t) = 2 +[7] frameRate (1 byte, uint8_t) = FPS +[8-15] reserved (8 bytes, all zeros) +``` + +### 3. Frame Data Block +For each frame (0 to frameCount-1), all motors are stored in the same order: + +``` +For each frame: + For each motor (in consistent order): + [motor_id: 1 byte, uint8_t] + [position: 2 bytes, uint16_t, little-endian, range 0-4095] +``` + +**Important**: +- All frames contain the same motors in the same order +- Motor count = (Frame Data Block size) / (frameCount * 3) +- Each motor record is exactly 3 bytes: 1 byte ID + 2 bytes position + +## Example File Layout + +For a file with 100 frames and 20 motors: + +``` +[0-1] Filename length (2 bytes) +[2-N] Filename (N bytes) +[N+0-N+3] "ANIM" (4 bytes) +[N+4-N+5] 100 (frameCount, 2 bytes) +[N+6] 2 (version, 1 byte) +[N+7] 24 (frameRate, 1 byte) +[N+8-N+15] Reserved (8 bytes) +[N+16+] Frame data: + Frame 0: [motor0_id][motor0_pos][motor1_id][motor1_pos]...[motor19_id][motor19_pos] + Frame 1: [motor0_id][motor0_pos][motor1_id][motor1_pos]...[motor19_id][motor19_pos] + ... + Frame 99: [motor0_id][motor0_pos][motor1_id][motor1_pos]...[motor19_id][motor19_pos] +``` + +Total frame data size = 100 frames × 20 motors × 3 bytes = 6,000 bytes diff --git a/HansonServo.ino b/HansonServo.ino index 7c78f31..439eded 100644 --- a/HansonServo.ino +++ b/HansonServo.ino @@ -83,31 +83,166 @@ void handleSerialPassthrough() { // Animation Playback // ============================================================================ -void runNodeAnimation() { +// Dispatcher: calls the appropriate animation function based on version +void runAnimation() { + if (!animState.current || !animState.current->isActive()) { + return; + } + + if (animState.current->header.version == 2) { + runFrameAnimation(); + } else { + runNodeAnimation(); + } +} + +// Version 2: Frame-by-frame animation playback +void runFrameAnimation() { static uint32_t lastTickTime = 0; static uint32_t currentTick = 0; - static bool wasActive = false; + static uint8_t lastGeneration = 0; if (!animState.current || !animState.current->isActive()) { - wasActive = false; return; } - // Reset tick when animation starts or if currentTick is less than startFrame - if (!wasActive || currentTick < animState.startFrame) { + // Reset tick when a new animation starts (detected by generation change) + if (lastGeneration != animState.playGeneration) { + currentTick = animState.startFrame; + lastTickTime = millis(); + lastGeneration = animState.playGeneration; + sendMessage("V2 Animation started, generation: " + String(lastGeneration) + ", startFrame: " + String(animState.startFrame)); + } + + config.enableAllMotors(); + + // Calculate frame interval from animation's frame rate + uint16_t frameIntervalMs = 1000 / animState.current->header.frameRate; + if (frameIntervalMs == 0) frameIntervalMs = 1; // Safety: prevent division by zero + + uint32_t now = millis(); + if (now - lastTickTime < frameIntervalMs) + return; + lastTickTime = now; + + // Get frame data for current tick + const std::vector* frameData = animState.current->getFrameData(currentTick); + + if (frameData && !frameData->empty()) { + // Collect motor commands from frame data + std::vector motorIDs; + std::vector positions; + std::vector speeds; + + for (const auto& motorPos : *frameData) { + motorIDs.push_back(motorPos.motorID); + positions.push_back(motorPos.position); + speeds.push_back(0); + config.setMotorPosition(motorPos.motorID, motorPos.position); + config.setMotorEnabled(motorPos.motorID, true); + } + + // Send all positions in one sync write + if (!motorIDs.empty()) { + servoManager.syncWritePositions(motorIDs.data(), positions.data(), + speeds.data(), motorIDs.size(), config, 0); + } + + // Debug: print frame and motor positions + // Serial.print("Frame "); + // Serial.print(currentTick); + // Serial.print(": "); + // for (size_t i = 0; i < motorIDs.size(); i++) { + // if (i > 0) Serial.print(", "); + // Serial.print("M"); + // Serial.print(motorIDs[i]); + // Serial.print("="); + // Serial.print(positions[i]); + // } + // Serial.println(); + } + + // Emit per-frame event + { + uint8_t payload[4]; + payload[0] = currentTick & 0xFF; + payload[1] = (currentTick >> 8) & 0xFF; + payload[2] = static_cast(animState.playMode); + payload[3] = 0; // in-progress + sendPacket(Tag::FRAME, payload, 4); + } + + currentTick++; + + // Handle animation end + uint16_t framesPlayed = currentTick - animState.startFrame; + uint16_t totalFrames = animState.current->getFrameCount(); + uint16_t remainingFrames = (totalFrames > animState.startFrame) ? (totalFrames - animState.startFrame) : 0; + + if (totalFrames > 0 && remainingFrames > 0 && framesPlayed >= remainingFrames) { + switch (animState.playMode) { + case PLAY_ONCE: + animState.stop(); + { + uint8_t done[4]; + done[0] = currentTick & 0xFF; + done[1] = (currentTick >> 8) & 0xFF; + done[2] = static_cast(animState.playMode); + done[3] = 1; // complete + //sendPacket(Tag::FRAME, done, 4); + } + break; + case PLAY_LOOP: + currentTick = animState.startFrame; + break; + case PLAY_REPEAT: + if (--animState.repeatsRemaining == 0) { + animState.stop(); + uint8_t done[4]; + done[0] = currentTick & 0xFF; + done[1] = (currentTick >> 8) & 0xFF; + done[2] = static_cast(animState.playMode); + done[3] = 1; // complete + //sendPacket(Tag::FRAME, done, 4); + } else { + currentTick = animState.startFrame; + } + break; + default: + break; + } + } +} + +// Version 1: Node graph animation playback +void runNodeAnimation() { + static uint32_t lastTickTime = 0; + static uint32_t currentTick = 0; + static uint8_t lastGeneration = 0; + + if (!animState.current || !animState.current->isActive()) { + return; + } + + // Reset tick when a new animation starts (detected by generation change) + if (lastGeneration != animState.playGeneration) { currentTick = animState.startFrame; // Start from specified frame lastTickTime = millis(); - wasActive = true; + lastGeneration = animState.playGeneration; // Debug: send startFrame via MSGE sendMessage("Animation startFrame: " + String(animState.startFrame) + ", currentTick: " + String(currentTick)); } config.enableAllMotors(); + // Calculate frame interval from animation's frame rate + uint16_t frameIntervalMs = 1000 / animState.current->header.frameRate; + if (frameIntervalMs == 0) frameIntervalMs = 1; // Safety: prevent division by zero + uint32_t now = millis(); - if (now - lastTickTime < FRAME_INTERVAL_MS) + if (now - lastTickTime < frameIntervalMs) return; - lastTickTime = now; + lastTickTime = now; // Tick the node graph animState.current->nodeGraph.tick(currentTick, *animState.current); @@ -294,6 +429,8 @@ void setup() { } Serial.println("[HansonServo] Filesystem ready"); + + // Load or create robot config if (config.loadOrCreateDefault()) { Serial.println("[HansonServo] Config loaded: " + config.deviceName); @@ -303,6 +440,26 @@ void setup() { Serial.println("[HansonServo] Ready"); Serial.println("[HansonServo] Protocol: 0xA5 0x5A tagged packets with CRC16"); + + // ---- TEST: Load and play animation ---- + // Serial.println("[TEST] Loading /slow.anim..."); + // if (animState.animation.loadFromFile("/slow.anim")) { + // Serial.println("[TEST] Animation loaded successfully"); + + // delay(1000); // Wait 1 second + + // // Print animation info + // Serial.println(animState.animation.printAnim()); + + // delay(5000); // Wait 5 seconds + + // // Play the animation + // Serial.println("[TEST] Playing animation..."); + // animState.play(PLAY_ONCE, 1, 0); + // } else { + // Serial.println("[TEST] Failed to load /animation.anim"); + // } + // ---- END TEST ---- } // ============================================================================ @@ -319,15 +476,15 @@ void loop() { // Protocol handling handleProtocol(); - // Animation playback - runNodeAnimation(); + // Animation playback (auto-selects v1 node or v2 frame based on version) + runAnimation(); // Motor position updates updateMotorPositions(); handleMotorStreaming(); // Sensor updates and streaming - sensors.update(); + //sensors.update(); // Heartbeat sendHeartbeat(); diff --git a/animation.cpp b/animation.cpp index 25c5f10..4ae0363 100644 --- a/animation.cpp +++ b/animation.cpp @@ -81,6 +81,25 @@ uint16_t Animation::getMotorPosition(uint8_t motorID, uint16_t timeCS) { void Animation::clear() { //memset(data, 0, sizeof(data)); + frameData.clear(); +} + +void Animation::setFrameData(uint16_t frameIndex, const std::vector& motors) { + if (frameIndex >= frameData.size()) { + frameData.resize(frameIndex + 1); + } + frameData[frameIndex] = motors; +} + +const std::vector* Animation::getFrameData(uint16_t frameIndex) const { + if (frameIndex >= frameData.size()) { + return nullptr; + } + return &frameData[frameIndex]; +} + +void Animation::clearFrameData() { + frameData.clear(); } // uint16_t* Animation::getRawData() { @@ -105,20 +124,32 @@ bool Animation::saveToFile(const char* filename) { file.write((uint8_t*)&header, sizeof(header)); - uint16_t curveCount = 0; - for (const auto& [motorID, segments] : curves) { - curveCount += segments.size(); - } - file.write((uint8_t*)&curveCount, sizeof(curveCount)); - for (const auto& [motorID, segments] : curves) { - for (const CurveSegment& seg : segments) { - file.write((uint8_t*)&seg, sizeof(CurveSegment)); + if (header.version == 2) { + // Version 2: Write frame data + // For each frame, write all motor positions + for (uint16_t frameIndex = 0; frameIndex < frameData.size() && frameIndex < header.frameCount; frameIndex++) { + const auto& frame = frameData[frameIndex]; + for (const auto& motorPos : frame) { + file.write((uint8_t*)&motorPos, sizeof(MotorPosition)); + } + } + } else { + // Version 1: Write curves and node graph + uint16_t curveCount = 0; + for (const auto& [motorID, segments] : curves) { + curveCount += segments.size(); + } + file.write((uint8_t*)&curveCount, sizeof(curveCount)); + for (const auto& [motorID, segments] : curves) { + for (const CurveSegment& seg : segments) { + file.write((uint8_t*)&seg, sizeof(CurveSegment)); + } } - } - // ✅ Write serialized node graph - std::vector graphData = nodeGraph.serialize(); - file.write(graphData.data(), graphData.size()); + // ✅ Write serialized node graph + std::vector graphData = nodeGraph.serialize(); + file.write(graphData.data(), graphData.size()); + } file.close(); return true; @@ -136,46 +167,87 @@ bool Animation::loadFromFile(const char* filename) { return false; } - if (strncmp(tempHeader.magic, "ANIM", 4) != 0 || tempHeader.version != 1) { + if (strncmp(tempHeader.magic, "ANIM", 4) != 0) { + file.close(); + return false; + } + + if (tempHeader.version != 1 && tempHeader.version != 2) { file.close(); return false; } header = tempHeader; - // Read curve count - uint16_t curveCount; - if (file.read((uint8_t*)&curveCount, sizeof(curveCount)) != sizeof(curveCount)) { - file.close(); - return false; - } - - clearAllCurves(); - - // Read curve segments - for (uint16_t i = 0; i < curveCount; i++) { - CurveSegment seg; - if (file.read((uint8_t*)&seg, sizeof(CurveSegment)) != sizeof(CurveSegment)) { + if (header.version == 2) { + // Version 2: Read frame data + clearFrameData(); + frameData.reserve(header.frameCount); + + // Calculate motor count from file size + size_t fileSize = file.size(); + size_t headerSize = sizeof(AnimationHeader); + size_t frameDataSize = fileSize - headerSize; + size_t frameSize = frameDataSize / header.frameCount; // bytes per frame + uint16_t motorCount = frameSize / sizeof(MotorPosition); // motors per frame + + if (frameSize % sizeof(MotorPosition) != 0) { file.close(); - return false; + return false; // Invalid frame data size } - curves[seg.motorID].push_back(seg); - } - - // ✅ Read remaining bytes into buffer - size_t remaining = file.available(); - if (remaining > 0) { - std::vector buffer(remaining); - if (file.read(buffer.data(), remaining) != remaining) { + + // Read all frames + for (uint16_t frameIndex = 0; frameIndex < header.frameCount; frameIndex++) { + std::vector frame; + frame.reserve(motorCount); + + for (uint16_t motorIndex = 0; motorIndex < motorCount; motorIndex++) { + MotorPosition motorPos; + if (file.read((uint8_t*)&motorPos, sizeof(MotorPosition)) != sizeof(MotorPosition)) { + file.close(); + return false; + } + frame.push_back(motorPos); + } + + frameData.push_back(frame); + } + } else { + // Version 1: Read curves and node graph + // Read curve count + uint16_t curveCount; + if (file.read((uint8_t*)&curveCount, sizeof(curveCount)) != sizeof(curveCount)) { file.close(); return false; } - // ✅ Load node graph from buffer - nodeGraph.nodes.clear(); - nodeGraph.connections.clear(); - loadNodeGraph(buffer.data(), buffer.size(), nodeGraph); - nodeGraph.bindAnimationContext(this); + clearAllCurves(); + + // Read curve segments + for (uint16_t i = 0; i < curveCount; i++) { + CurveSegment seg; + if (file.read((uint8_t*)&seg, sizeof(CurveSegment)) != sizeof(CurveSegment)) { + file.close(); + return false; + } + curves[seg.motorID].push_back(seg); + } + + // ✅ Read remaining bytes into buffer + size_t remaining = file.available(); + if (remaining > 0) { + std::vector buffer(remaining); + if (file.read(buffer.data(), remaining) != remaining) { + file.close(); + return false; + } + + // ✅ Load node graph from buffer + nodeGraph.nodes.clear(); + nodeGraph.connections.clear(); + loadNodeGraph(buffer.data(), buffer.size(), nodeGraph); + nodeGraph.bindAnimationContext(this); + } } file.close(); @@ -211,6 +283,54 @@ String Animation::printCurves() { return output; } +String Animation::printAnim() { + String output = "ANIMATION INFO\n"; + output += "==============\n"; + output += "Version: " + String(header.version) + "\n"; + output += "Frame Count: " + String(header.frameCount) + "\n"; + output += "Frame Rate: " + String(header.frameRate) + " fps\n"; + + if (header.frameRate > 0) { + float duration = (float)header.frameCount / (float)header.frameRate; + output += "Duration: " + String(duration, 2) + " seconds\n"; + } + + output += "Active: " + String(isActive() ? "Yes" : "No") + "\n"; + + if (header.version == 1) { + // Version 1: curves and node graph + uint16_t curveCount = 0; + for (const auto& [motorID, segments] : curves) { + curveCount += segments.size(); + } + output += "Curve Segments: " + String(curveCount) + "\n"; + output += "Motors with Curves: " + String(curves.size()) + "\n"; + output += "Node Graph Nodes: " + String(nodeGraph.nodes.size()) + "\n"; + output += "Node Graph Connections: " + String(nodeGraph.connections.size()) + "\n"; + } else if (header.version == 2) { + // Version 2: frame data + if (!frameData.empty()) { + uint16_t motorCount = frameData[0].size(); + output += "Motors per Frame: " + String(motorCount) + "\n"; + output += "Frames Stored: " + String(frameData.size()) + "\n"; + + // Show motor IDs from first frame + if (!frameData[0].empty()) { + output += "Motor IDs: "; + for (size_t i = 0; i < frameData[0].size(); i++) { + if (i > 0) output += ", "; + output += String(frameData[0][i].motorID); + } + output += "\n"; + } + } else { + output += "Frame Data: Empty\n"; + } + } + + return output; +} + diff --git a/animation.h b/animation.h index 000a56d..c884bb4 100644 --- a/animation.h +++ b/animation.h @@ -4,6 +4,7 @@ #include "FS.h" #include "FFat.h" #include +#include #include "nodegraph.h" @@ -34,6 +35,12 @@ struct __attribute__((packed)) CurveSegment { int16_t endPointY; }; +// Version 2 frame data: motor ID + position pair +struct __attribute__((packed)) MotorPosition { + uint8_t motorID; + uint16_t position; // 0-4095 +}; + @@ -51,8 +58,14 @@ public: void clearCurves(uint8_t motorID); void clearAllCurves(); String printCurves(); + String printAnim(); uint16_t getMotorPosition(uint8_t motorID, uint16_t timeCS); + // Version 2 frame data methods + void setFrameData(uint16_t frameIndex, const std::vector& motors); + const std::vector* getFrameData(uint16_t frameIndex) const; + void clearFrameData(); + void clear(); //uint16_t* getRawData(); // Optional: for bulk access //size_t getSize() const; @@ -68,7 +81,8 @@ public: private: //uint16_t data[MAX_FRAMES][NUM_CHANNELS]; - std::unordered_map> curves; + std::unordered_map> curves; // Version 1: curves + std::vector> frameData; // Version 2: raw frame data bool active = false; }; diff --git a/commands.cpp b/commands.cpp index 6d112c9..b185d7a 100644 --- a/commands.cpp +++ b/commands.cpp @@ -48,6 +48,7 @@ void AnimationState::play(PlayMode mode, uint8_t repeats, uint16_t startFrame) { playMode = mode; repeatsRemaining = repeats; this->startFrame = startFrame; + playGeneration++; // Signal that a new animation has started } void AnimationState::stop() { @@ -645,28 +646,59 @@ bool parseAndSaveAnimation(const uint8_t* payload, uint16_t len, Animation& anim ptr += 16; remaining -= 16; - // Curve count (at start of curve block) - if (remaining < 2) return false; - uint16_t curveCount = ptr[0] | (ptr[1] << 8); - ptr += 2; - remaining -= 2; + 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 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; + // 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); + // Node graph (whatever remains) + if (remaining > 0) { + loadNodeGraph(ptr, remaining, animation.nodeGraph); + animation.nodeGraph.bindAnimationContext(&animation); + } } // Save to file diff --git a/commands.h b/commands.h index ca882d5..460b104 100644 --- a/commands.h +++ b/commands.h @@ -16,6 +16,7 @@ struct AnimationState { PlayMode playMode = PLAY_IDLE; uint8_t repeatsRemaining = 0; uint16_t startFrame = 0; // Frame to start playback from + uint8_t playGeneration = 0; // Increments each time play() is called void play(PlayMode mode, uint8_t repeats = 0, uint16_t startFrame = 0); void stop();