From 06883652d0fb2d75dadbb12deeec625332a52e7a Mon Sep 17 00:00:00 2001 From: Jake Date: Wed, 8 Oct 2025 15:38:44 +0800 Subject: [PATCH] animation files now made up of curves --- HansonServo.ino | 57 +++++++++++- animation.cpp | 230 +++++++++++++++++++++++++++++++++++++++++------- animation.h | 26 +++--- 3 files changed, 271 insertions(+), 42 deletions(-) diff --git a/HansonServo.ino b/HansonServo.ino index dd25071..38994b8 100644 --- a/HansonServo.ino +++ b/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; diff --git a/animation.cpp b/animation.cpp index 04aa5da..af8ded7 100644 --- a/animation.cpp +++ b/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); - float p1 = toFloat(seg.startHandle); - float p2 = toFloat(seg.endHandle); - float p3 = toFloat(seg.endPoint); + // Define control points + float x0 = seg.startTime; + float x1 = seg.startHandleX; + 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; + return u * u * u * x0 + 3 * u * u * t * x1 + 3 * u * t * t * x2 + t * t * t * x3; + }; + + 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*p0 + 3*u*u*t*p1 + 3*u*t*t*p2 + t*t*t*p3; + float value = u * u * u * y0 + 3 * u * u * t * y1 + 3 * u * t * t * y2 + t * t * t * y3; - // Remap back to 0–4095 for PWM + // 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() { - return &data[0][0]; -} +// uint16_t* Animation::getRawData() { +// return &data[0][0]; +// } -size_t Animation::getSize() const { - return sizeof(data); +// size_t Animation::getSize() const { +// 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 (uint8_t ch = 0; ch < NUM_CHANNELS; ch++) { - if (data[frame][ch] != 0) { - lastFrame = frame; - break; - } - } - } - header.frameCount = lastFrame + 1; + // for (uint16_t frame = 0; frame < MAX_FRAMES; frame++) { + // for (uint8_t ch = 0; ch < NUM_CHANNELS; ch++) { + // if (data[frame][ch] != 0) { + // lastFrame = frame; + // break; + // } + // } + // } + //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); - if (file.read((uint8_t*)data, expectedSize) != expectedSize) { - file.close(); - return false; - } + // size_t expectedSize = sizeof(data); + // if (file.read((uint8_t*)data, expectedSize) != expectedSize) { + // file.close(); + // 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); +} diff --git a/animation.h b/animation.h index 86bd49d..48ad797 100644 --- a/animation.h +++ b/animation.h @@ -22,13 +22,15 @@ struct AnimationHeader { struct __attribute__((packed)) CurveSegment { uint8_t motorID; - uint16_t startTime; // centiseconds (0.01s) MAX 655.35 seconds - uint16_t endTime; + uint16_t startTime;// centiseconds (0.01s) MAX 655.35 seconds + uint16_t endTime;// centiseconds (0.01s) MAX 655.35 seconds // remapped from -1 to 1 → 0–65535 - uint16_t startPoint; // start value - uint16_t startHandle; // start handle - uint16_t endPoint; // end value - uint16_t endHandle; // end handle + uint16_t startPointY; + uint16_t startHandleX; + uint16_t startHandleY; + 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 - size_t getSize() const; + //uint16_t* getRawData(); // Optional: for bulk access + //size_t getSize() const; bool saveToFile(const char* filename); bool loadFromFile(const char* filename); uint16_t getFrameCount() const; - void createSampleSweep(uint8_t seconds); - void createStaggeredSweep(uint8_t seconds); + void setFrameCount(uint16_t count); + void createBasicSCurve(); + void createEaseOutCurve(); AnimationHeader header; private: - uint16_t data[MAX_FRAMES][NUM_CHANNELS]; + //uint16_t data[MAX_FRAMES][NUM_CHANNELS]; std::vector curves[NUM_CHANNELS]; // One list per motor channel };