From 366ba5fae3468b6b99dd4138421ff92b732109bc Mon Sep 17 00:00:00 2001
From: Jake <realrobotshk@gmail.com>
Date: Tue, 30 Sep 2025 00:12:38 +0800
Subject: [PATCH] can play animation files from control panel, animation now
 via keyframe, not individual frames

---
 HansonServo.ino | 103 +++++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 102 insertions(+), 1 deletion(-)

diff --git a/HansonServo.ino b/HansonServo.ino
index 944b3e7..875cacc 100644
--- a/HansonServo.ino
+++ b/HansonServo.ino
@@ -18,6 +18,7 @@ uint8_t payload[MAX_PAYLOAD_SIZE];  // Global or static
 #define CMD_SAVE_FILE 0x05
 #define CMD_MESSAGE 0x06
 #define CMD_SET_POSITION 0x07
+#define CMD_PLAY_FILE 0x08
 
 
 // ESP32 S2 PINOUT
@@ -57,7 +58,9 @@ void setup() {
     Serial.println("FFat mount failed");
     return;
   }
-
+  anim.loadFromFile("/bob.anim");
+  anim.printKeyframes();
+  playAnimation(anim);
   // ClearFiles();
   // // PrintFileList();
   // sweep.clear();
@@ -171,6 +174,9 @@ void handleCommand(uint8_t command, const uint8_t* payload, uint16_t length) {
       handleSetPosition(payload, length);
       break;
 
+    case CMD_PLAY_FILE:
+      handlePlayAnimation(payload, length);
+
     default:
       Serial.print("Unknown command: ");
       Serial.println(command, HEX);
@@ -341,6 +347,38 @@ void handleDeleteFile(const uint8_t* payload, uint16_t length) {
   sendMessage(("File Deleted: " + String(filename)).c_str(), CMD_DELETE_FILE);
 }
 
+
+void handlePlayAnimation(const uint8_t* payload, uint16_t length) {
+  sendMessage("Playing FILE");
+  if (length < 1) {
+    Serial.println("Payload too short for filename length");
+    sendMessage("Payload too short for filename length");
+    return;
+  }
+
+  // 🔹 Parse filename
+  uint16_t filenameLength = payload[0] | (payload[1] << 8);
+  if (length < 2 + filenameLength) {
+    Serial.println("Payload too short for filename");
+    sendMessage("Payload too short for filename");
+    return;
+  }
+
+
+  char filename[filenameLength + 1];
+
+
+  memcpy(filename, payload + 2, filenameLength);
+  filename[filenameLength] = '\0';
+
+  //deleteFile(FFat, ("/" + String(filename)).c_str());
+  anim.clear();
+  anim.loadFromFile(("/" + String(filename)).c_str());
+  playAnimation(anim);
+
+  sendMessage("File Played", CMD_PLAY_FILE);
+}
+
 void handleSaveFile(const uint8_t* payload, uint16_t length) {
   bool valid = parseAnimationPayload(payload, length, anim);
 
@@ -574,6 +612,69 @@ void deleteFile(fs::FS& fs, const char* path) {
 }
 
 void playAnimation(Animation& anim) {
+  uint16_t positions[NUM_CHANNELS];
+  const uint16_t frameCount = 400;//anim.getFrameCount();
+  const uint32_t frameDelay = 1000 / FRAMES_PER_SECOND;
+  uint32_t nextFrameTime = millis();
+  Serial.print("Frame Count: ");
+  Serial.println(frameCount);
+
+  // Organize keyframes per motor
+  std::vector<Keyframe> motorKeyframes[NUM_CHANNELS];
+  for (const auto& kf : anim.getKeyframes()) {
+    if (kf.motorId < NUM_CHANNELS) {
+      motorKeyframes[kf.motorId].push_back(kf);
+    }
+  }
+
+  // Sort keyframes per motor by frame
+  for (int ch = 0; ch < NUM_CHANNELS; ch++) {
+    std::sort(motorKeyframes[ch].begin(), motorKeyframes[ch].end(),
+              [](const Keyframe& a, const Keyframe& b) {
+                return a.frame < b.frame;
+              });
+  }
+
+  for (uint16_t frame = 0; frame < frameCount; frame++) {
+    while (millis() < nextFrameTime) {
+      delay(1);
+    }
+
+    for (int ch = 0; ch < NUM_CHANNELS; ch++) {
+      const auto& kfs = motorKeyframes[ch];
+      uint16_t value = 512;  // default position
+
+      // Find surrounding keyframes
+      Keyframe prev = { ch, 0, 512 }, next = { ch, frameCount, 512 };
+      for (size_t i = 0; i < kfs.size(); i++) {
+        if (kfs[i].frame <= frame) {
+          prev = kfs[i];
+        }
+        if (kfs[i].frame > frame) {
+          next = kfs[i];
+          break;
+        }
+      }
+
+      // Interpolate
+      if (prev.frame == next.frame) {
+        value = prev.position;
+      } else {
+        float t = float(frame - prev.frame) / (next.frame - prev.frame);
+        value = prev.position + t * (next.position - prev.position);
+      }
+
+      positions[ch] = value;
+    }
+    Serial.println(positions[0]);
+    servos.syncWritePos(ids, positions, NUM_CHANNELS);
+    nextFrameTime += frameDelay;
+  }
+}
+
+
+
+void playAnimationOLD(Animation& anim) {
   uint16_t positions[NUM_CHANNELS];
   const uint16_t frameCount = anim.getFrameCount();
   const uint32_t frameDelay = 1000 / FRAMES_PER_SECOND;  // 20 ms