animation files now made up of curves

HansonServo.ino

@@ -91,6 +91,22 @@ void setup() {
     Serial.println("FFat mount failed");
     return;
   }
+
+
+  
+  // anim.saveToFile("/scurve.anim");
+  // Serial.println("SAVED");
+  //anim.loadFromFile("/scurve.anim");
+  //anim.createBasicSCurve();
+  // Serial.println("loading");
+   //anim.saveToFile("/pointcurve.anim");
+   //playAnimation(anim);
+  // Serial.println("DONE");
+  // anim.createEaseOutCurve();
+  // playAnimation(anim);
+
+  //anim.printCurves();
+  // PrintFileList();
   // anim.loadFromFile("/bob.anim");
   // anim.printKeyframes();
   // playAnimation(anim);
@@ -889,7 +905,46 @@ void deleteFile(fs::FS& fs, const char* path) {
   }
 }
 
-void playAnimation(Animation& anim) {
+
+void playAnimation(Animation& animation) {
+ uint16_t durationCS = animation.getFrameCount();
+  const uint8_t fps = 48;
+  const uint32_t frameIntervalMS = 1000 / fps;  // ~20.83 ms
+  const uint32_t totalDurationMS = durationCS * 10;
+
+  uint32_t startTime = millis();
+  uint32_t nextFrameTime = startTime;
+
+  while (millis() - startTime < totalDurationMS) {
+    uint32_t currentTime = millis();
+
+    if (currentTime >= nextFrameTime) {
+      uint16_t timeCS = (currentTime - startTime) / 10;
+
+      //for (uint8_t motorID = 0; motorID < NUM_CHANNELS; motorID++) {
+        uint16_t pos = animation.getMotorPosition(0, timeCS);
+        pos = pos / 4;
+        servos[0]->sendWritePos(10, pos);
+        
+        Serial.println(pos);
+      //}
+
+      nextFrameTime += frameIntervalMS;
+    }
+
+    // Optional: yield or small delay to avoid busy loop
+    delay(1);
+  }
+
+  // Optional: reset motors to center
+  // for (uint8_t motorID = 0; motorID < NUM_CHANNELS; motorID++) {
+  //   servos[0]->sendWritePos(motorID, 2048);
+  // }
+}
+
+
+
+void playAnimationOLD(Animation& anim) {
   // uint16_t positions[NUM_CHANNELS];
   // const uint16_t frameCount = 400;  //anim.getFrameCount();
   // const uint32_t frameDelay = 1000 / FRAMES_PER_SECOND;

animation.cpp

@@ -32,41 +32,69 @@ uint16_t Animation::getMotorPosition(uint8_t motorID, uint16_t timeCS) {
 
   for (const auto& seg : curves[motorID]) {
     if (timeCS >= seg.startTime && timeCS <= seg.endTime) {
-      float t = float(timeCS - seg.startTime) / (seg.endTime - seg.startTime);
-
       // Convert uint16_t to float in range -1 to 1
       auto toFloat = [](uint16_t v) {
         return (float(v) / 65535.0f) * 2.0f - 1.0f;
       };
 
-      float p0 = toFloat(seg.startPoint);
+      // Define control points
-      float p1 = toFloat(seg.startHandle);
+      float x0 = seg.startTime;
-      float p2 = toFloat(seg.endHandle);
+      float x1 = seg.startHandleX;
-      float p3 = toFloat(seg.endPoint);
+      float x2 = seg.endHandleX;
+      float x3 = seg.endTime;
 
+      float y0 = toFloat(seg.startPointY);
+      float y1 = toFloat(seg.startHandleY);
+      float y2 = toFloat(seg.endHandleY);
+      float y3 = toFloat(seg.endPointY);
+
+      // Solve for t such that Bézier x(t) ≈ timeCS
+      auto bezierX = [&](float t) {
         float u = 1.0f - t;
-      float value = u*u*u*p0 + 3*u*u*t*p1 + 3*u*t*t*p2 + t*t*t*p3;
+        return u * u * u * x0 + 3 * u * u * t * x1 + 3 * u * t * t * x2 + t * t * t * x3;
+      };
 
-      // Remap back to 0–4095 for PWM
+      float t = 0.5f;
+      float lower = 0.0f;
+      float upper = 1.0f;
+
+      for (int i = 0; i < 20; ++i) {
+        float x = bezierX(t);
+        if (fabs(x - timeCS) < 0.5f) break;
+        if (x < timeCS) lower = t;
+        else upper = t;
+        t = (lower + upper) * 0.5f;
+      }
+
+      // Evaluate Bézier y(t)
+      float u = 1.0f - t;
+      float value = u * u * u * y0 + 3 * u * u * t * y1 + 3 * u * t * t * y2 + t * t * t * y3;
+
+      // Remap to PWM range
       return constrain((value + 1.0f) * 2047.5f, 0, 4095);
     }
   }
 
-  return 2048; // Default center if no segment matches
+  return 2048;  // Default center
 }
 
 
 
+
 void Animation::clear() {
-  memset(data, 0, sizeof(data));  
+  //memset(data, 0, sizeof(data));
 }
 
-uint16_t* Animation::getRawData() {
+// uint16_t* Animation::getRawData() {
-  return &data[0][0];
+//   return &data[0][0];
-}
+// }
 
-size_t Animation::getSize() const {
+// size_t Animation::getSize() const {
-  return sizeof(data);
+//   return sizeof(data);
+// }
+
+void Animation::setFrameCount(uint16_t count) {
+  header.frameCount = count;
 }
 
 uint16_t Animation::getFrameCount() const {
@@ -76,22 +104,22 @@ uint16_t Animation::getFrameCount() const {
 bool Animation::saveToFile(const char* filename) {
   // Auto-detect actual frame count
   uint16_t lastFrame = 0;
-  for (uint16_t frame = 0; frame < MAX_FRAMES; frame++) {
+  // for (uint16_t frame = 0; frame < MAX_FRAMES; frame++) {
-    for (uint8_t ch = 0; ch < NUM_CHANNELS; ch++) {
+  //   for (uint8_t ch = 0; ch < NUM_CHANNELS; ch++) {
-      if (data[frame][ch] != 0) {
+  //     if (data[frame][ch] != 0) {
-        lastFrame = frame;
+  //       lastFrame = frame;
-        break;
+  //       break;
-      }
+  //     }
-    }
+  //   }
-  }
+  // }
-  header.frameCount = lastFrame + 1;
+  //header.frameCount = lastFrame + 1;
 
   File file = FFat.open(filename, FILE_WRITE);
   if (!file) return false;
 
   // Write header and motion data
   file.write((uint8_t*)&header, sizeof(header));
-  file.write((uint8_t*)data, sizeof(data));
+  //file.write((uint8_t*)data, sizeof(data));
 
   // Count total curve segments
   uint16_t curveCount = 0;
@@ -113,6 +141,7 @@ bool Animation::saveToFile(const char* filename) {
   return true;
 }
 
+
 bool Animation::loadFromFile(const char* filename) {
   File file = FFat.open(filename, FILE_READ);
   if (!file) return false;
@@ -132,11 +161,11 @@ bool Animation::loadFromFile(const char* filename) {
   header = tempHeader;
 
   // Read motion data
-  size_t expectedSize = sizeof(data);
+  // size_t expectedSize = sizeof(data);
-  if (file.read((uint8_t*)data, expectedSize) != expectedSize) {
+  // if (file.read((uint8_t*)data, expectedSize) != expectedSize) {
-    file.close();
+  //   file.close();
-    return false;
+  //   return false;
-  }
+  // }
 
   // Read curve count
   uint16_t curveCount;
@@ -165,3 +194,144 @@ bool Animation::loadFromFile(const char* filename) {
   return true;
 }
 
+
+void Animation::printCurves() {
+  Serial.println("=== Curve Segments ===");
+
+  auto toFloat = [](uint16_t v) -> float {
+    return (float(v) / 65535.0f) * 2.0f - 1.0f;
+  };
+
+  for (uint8_t motorID = 0; motorID < NUM_CHANNELS; motorID++) {
+    if (curves[motorID].empty()) continue;
+
+    Serial.print("Motor ");
+    Serial.print(motorID);
+    Serial.println(":");
+
+    for (size_t i = 0; i < curves[motorID].size(); ++i) {
+      const CurveSegment& seg = curves[motorID][i];
+
+      Serial.print("  Segment ");
+      Serial.print(i);
+      Serial.print(" | Time: ");
+      Serial.print(seg.startTime * 0.01f, 2);
+      Serial.print("s → ");
+      Serial.print(seg.endTime * 0.01f, 2);
+      Serial.println("s");
+
+      Serial.print("    Start Point Y: ");
+      Serial.println(toFloat(seg.startPointY), 3);
+
+      Serial.print("    Start Handle: (");
+      Serial.print(seg.startHandleX * 0.01f, 2);
+      Serial.print("s, ");
+      Serial.print(toFloat(seg.startHandleY), 3);
+      Serial.println(")");
+
+      Serial.print("    End Handle:   (");
+      Serial.print(seg.endHandleX * 0.01f, 2);
+      Serial.print("s, ");
+      Serial.print(toFloat(seg.endHandleY), 3);
+      Serial.println(")");
+
+      Serial.print("    End Point Y: ");
+      Serial.println(toFloat(seg.endPointY), 3);
+    }
+  }
+
+  Serial.println("======================");
+}
+
+
+
+void Animation::createBasicSCurve() {
+  clearAllCurves();
+
+  // Helper to convert float [-1, 1] to uint16_t [0, 65535]
+  auto toUint16 = [](float v) -> uint16_t {
+    return constrain((v + 1.0f) * 32767.5f, 0, 65535);
+  };
+
+  setFrameCount(800);  // 8.00 seconds
+
+  // First segment: -1 at 0s → +1 at 0.40s
+  CurveSegment seg1;
+  seg1.motorID = 0;
+  seg1.startTime = 0;  // 0.00s
+  seg1.endTime = 40;   // 0.40s = 40 centiseconds
+
+  seg1.startPointY = toUint16(-1.0f);  // P0.y
+  seg1.startHandleX = 10;              // P1.x (early pull)
+  seg1.startHandleY = toUint16(-1.0f);
+
+  seg1.endHandleX = 30;  // P2.x (late pull)
+  seg1.endHandleY = toUint16(+1.0f);
+
+  seg1.endPointY = toUint16(+1.0f);  // P3.y
+
+  addCurveSegment(seg1);
+
+  // Second segment: +1 at 0.40s → -1 at 8.00s
+  CurveSegment seg2;
+  seg2.motorID = 0;
+  seg2.startTime = 40;  // 0.40s
+  seg2.endTime = 800;   // 8.00s
+
+  seg2.startPointY = toUint16(+1.0f);  // P0.y
+  seg2.startHandleX = 200;             // P1.x (early pull)
+  seg2.startHandleY = toUint16(+1.0f);
+
+  seg2.endHandleX = 600;  // P2.x (late pull)
+  seg2.endHandleY = toUint16(-1.0f);
+
+  seg2.endPointY = toUint16(-1.0f);  // P3.y
+
+  addCurveSegment(seg2);
+}
+
+
+void Animation::createEaseOutCurve() {
+  clearAllCurves();
+
+  // Helper to convert float [-1, 1] to uint16_t [0, 65535]
+  auto toUint16 = [](float v) -> uint16_t {
+    return constrain((v + 1.0f) * 32767.5f, 0, 65535);
+  };
+
+  setFrameCount(400);  // 8.00 seconds total
+
+  // Segment 1: Ease out from -1 to +1 over 4 seconds
+  CurveSegment seg;
+  seg.motorID = 0;
+  seg.startTime = 0;  // 0.00s
+  seg.endTime = 200;  // 4.00s
+
+  seg.startPointY = toUint16(-1.0f);  // P0.y
+  seg.startHandleX = 50;              // P1.x (early pull → slow start)
+  seg.startHandleY = toUint16(-1.0f);
+
+  seg.endHandleX = 180;  // P2.x (late pull → fast finish)
+  seg.endHandleY = toUint16(+0.5f);
+
+  seg.endPointY = toUint16(+1.0f);  // P3.y
+
+  addCurveSegment(seg);
+
+  // Segment 2: Ease out from +1 to -1 over 4 seconds
+  CurveSegment returnSeg;
+  returnSeg.motorID = 0;
+  returnSeg.startTime = 200;  // 4.00s
+  returnSeg.endTime = 400;    // 8.00s
+
+  returnSeg.startPointY = toUint16(+1.0f);  // P0.y
+  returnSeg.startHandleX = 250;             // P1.x (early pull → slow start)
+  returnSeg.startHandleY = toUint16(+1.0f);
+
+  returnSeg.endHandleX = 380;  // P2.x (late pull → fast finish)
+  returnSeg.endHandleY = toUint16(-0.5f);
+
+  returnSeg.endPointY = toUint16(-1.0f);  // P3.y
+
+  addCurveSegment(returnSeg);
+}

animation.h

@@ -23,12 +23,14 @@ struct AnimationHeader {
 struct __attribute__((packed)) CurveSegment {
   uint8_t motorID;
   uint16_t startTime;// centiseconds (0.01s) MAX 655.35 seconds
-  uint16_t endTime;
+  uint16_t endTime;// centiseconds (0.01s) MAX 655.35 seconds
   // remapped from -1 to 1 → 0–65535
-  uint16_t startPoint;   // start value
+  uint16_t startPointY;
-  uint16_t startHandle;  // start handle
+  uint16_t startHandleX;
-  uint16_t endPoint;     // end value
+  uint16_t startHandleY;
-  uint16_t endHandle;    // end handle
+  uint16_t endHandleX;
+  uint16_t endHandleY;
+  uint16_t endPointY;
 };
 
 
@@ -45,20 +47,22 @@ public:
   void addCurveSegment(const CurveSegment& segment);
   void clearCurves(uint8_t motorID);
   void clearAllCurves();
+  void printCurves();
   uint16_t getMotorPosition(uint8_t motorID, uint16_t timeCS);
   
   void clear();
-  uint16_t* getRawData();  // Optional: for bulk access
+  //uint16_t* getRawData();  // Optional: for bulk access
-  size_t getSize() const;
+  //size_t getSize() const;
   bool saveToFile(const char* filename);
   bool loadFromFile(const char* filename);
   uint16_t getFrameCount() const;
-  void createSampleSweep(uint8_t seconds);
+  void setFrameCount(uint16_t count);
-  void createStaggeredSweep(uint8_t seconds);
+  void createBasicSCurve();
+  void createEaseOutCurve();
   AnimationHeader header;
 
 private:
-  uint16_t data[MAX_FRAMES][NUM_CHANNELS];
+  //uint16_t data[MAX_FRAMES][NUM_CHANNELS];
   std::vector<CurveSegment> curves[NUM_CHANNELS]; // One list per motor channel
 
 };