animation file v2 implemented, might break old connections. playback at fps defined by animation file

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

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
+  // Reset tick when a new animation starts (detected by generation change)
-  if (!wasActive || currentTick < animState.startFrame) {
+  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<MotorPosition>* frameData = animState.current->getFrameData(currentTick);
+  
+  if (frameData && !frameData->empty()) {
+    // Collect motor commands from frame data
+    std::vector<uint8_t> motorIDs;
+    std::vector<uint16_t> positions;
+    std::vector<uint16_t> 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<uint8_t>(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<uint8_t>(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<uint8_t>(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
+  // Animation playback (auto-selects v1 node or v2 frame based on version)
-  runNodeAnimation();
+  runAnimation();
 
   // Motor position updates
   updateMotorPositions();
   handleMotorStreaming();
 
   // Sensor updates and streaming
-  sensors.update();
+  //sensors.update();
 
   // Heartbeat
   sendHeartbeat();

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<MotorPosition>& motors) {
+  if (frameIndex >= frameData.size()) {
+    frameData.resize(frameIndex + 1);
+  }
+  frameData[frameIndex] = motors;
+}
+
+const std::vector<MotorPosition>* 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;
+  if (header.version == 2) {
-  for (const auto& [motorID, segments] : curves) {
+    // Version 2: Write frame data
-    curveCount += segments.size();
+    // For each frame, write all motor positions
-  }
+    for (uint16_t frameIndex = 0; frameIndex < frameData.size() && frameIndex < header.frameCount; frameIndex++) {
-  file.write((uint8_t*)&curveCount, sizeof(curveCount));
+      const auto& frame = frameData[frameIndex];
-  for (const auto& [motorID, segments] : curves) {
+      for (const auto& motorPos : frame) {
-    for (const CurveSegment& seg : segments) {
+        file.write((uint8_t*)&motorPos, sizeof(MotorPosition));
-      file.write((uint8_t*)&seg, sizeof(CurveSegment));
+      }
+    }
+  } 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
+    // ✅ Write serialized node graph
-  std::vector<uint8_t> graphData = nodeGraph.serialize();
+    std::vector<uint8_t> graphData = nodeGraph.serialize();
-  file.write(graphData.data(), graphData.size());
+    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
+  if (header.version == 2) {
-  uint16_t curveCount;
+    // Version 2: Read frame data
-  if (file.read((uint8_t*)&curveCount, sizeof(curveCount)) != sizeof(curveCount)) {
+    clearFrameData();
-    file.close();
+    frameData.reserve(header.frameCount);
-    return false;
+    
-  }
+    // Calculate motor count from file size
-
+    size_t fileSize = file.size();
-  clearAllCurves();
+    size_t headerSize = sizeof(AnimationHeader);
-
+    size_t frameDataSize = fileSize - headerSize;
-  // Read curve segments
+    size_t frameSize = frameDataSize / header.frameCount;  // bytes per frame
-  for (uint16_t i = 0; i < curveCount; i++) {
+    uint16_t motorCount = frameSize / sizeof(MotorPosition);  // motors per frame
-    CurveSegment seg;
+    
-    if (file.read((uint8_t*)&seg, sizeof(CurveSegment)) != sizeof(CurveSegment)) {
+    if (frameSize % sizeof(MotorPosition) != 0) {
       file.close();
-      return false;
+      return false;  // Invalid frame data size
     }
-    curves[seg.motorID].push_back(seg);
+    
-  }
+    // Read all frames
-
+    for (uint16_t frameIndex = 0; frameIndex < header.frameCount; frameIndex++) {
-  // ✅ Read remaining bytes into buffer
+      std::vector<MotorPosition> frame;
-  size_t remaining = file.available();
+      frame.reserve(motorCount);
-  if (remaining > 0) {
+      
-    std::vector<uint8_t> buffer(remaining);
+      for (uint16_t motorIndex = 0; motorIndex < motorCount; motorIndex++) {
-    if (file.read(buffer.data(), remaining) != remaining) {
+        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
+    clearAllCurves();
-    nodeGraph.nodes.clear();
+
-    nodeGraph.connections.clear();
+    // Read curve segments
-    loadNodeGraph(buffer.data(), buffer.size(), nodeGraph);
+    for (uint16_t i = 0; i < curveCount; i++) {
-    nodeGraph.bindAnimationContext(this);
+      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<uint8_t> 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;
+}
+
 
 
 

animation.h

@@ -4,6 +4,7 @@
 #include "FS.h"
 #include "FFat.h"
 #include <unordered_map>
+#include <vector>
 #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<MotorPosition>& motors);
+  const std::vector<MotorPosition>* 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<uint8_t, std::vector<CurveSegment>> curves;
+  std::unordered_map<uint8_t, std::vector<CurveSegment>> curves;  // Version 1: curves
+  std::vector<std::vector<MotorPosition>> frameData;  // Version 2: raw frame data
   bool active = false;
 };
 

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 (animation.header.version == 2) {
-  if (remaining < 2) return false;
+    // Version 2: Frame data block
-  uint16_t curveCount = ptr[0] | (ptr[1] << 8);
+    // Calculate motor count from remaining data
-  ptr += 2;
+    if (animation.header.frameCount == 0) return false;
-  remaining -= 2;
+    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<MotorPosition> 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)
+    // 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;
-    memcpy(&seg, ptr, sizeof(CurveSegment));
+      memcpy(&seg, ptr, sizeof(CurveSegment));
-    animation.addCurveSegment(seg);
+      animation.addCurveSegment(seg);
-    ptr += sizeof(CurveSegment);
+      ptr += sizeof(CurveSegment);
-  }
+    }
-  remaining -= curveDataSize;
+    remaining -= curveDataSize;
 
-  // Node graph (whatever remains)
+    // 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

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();