From 22cdb340631eb828b7942f05cde349ec0679496d Mon Sep 17 00:00:00 2001 From: Jake Date: Tue, 7 Oct 2025 14:26:49 +0800 Subject: [PATCH] adapted for curves rather than keyframes (untested) --- HansonServo.ino | 147 +++++++++++++++-------------------- animation.cpp | 200 ++++++++++++++++-------------------------------- animation.h | 24 ++++-- 3 files changed, 145 insertions(+), 226 deletions(-) diff --git a/HansonServo.ino b/HansonServo.ino index b51b111..dd25071 100644 --- a/HansonServo.ino +++ b/HansonServo.ino @@ -576,23 +576,22 @@ bool parseAnimationPayload(const uint8_t* payload, uint16_t length, Animation& a animation.header.frameRate = ptr[7]; memcpy(animation.header.reserved, ptr + 8, 8); - uint16_t keyframeCount = ptr[16] | (ptr[17] << 8); + uint16_t curveCount = ptr[16] | (ptr[17] << 8); ptr += 18; - if (length < (ptr - payload) + keyframeCount * 5) { - Serial.println("Payload too short for keyframes"); + if (length < (ptr - payload) + curveCount * sizeof(CurveSegment)) { + Serial.println("Payload too short for curve segments"); sendMessage("Payload too short"); return false; } - // 🔹 Parse keyframes - animation.clear(); - for (uint16_t i = 0; i < keyframeCount; i++) { - uint8_t motorId = ptr[0]; - uint16_t frame = ptr[1] | (ptr[2] << 8); - uint16_t position = ptr[3] | (ptr[4] << 8); - animation.addKeyframe(motorId, frame, position); - ptr += 5; + // 🔹 Parse curve segments + animation.clearAllCurves(); + for (uint16_t i = 0; i < curveCount; i++) { + CurveSegment seg; + memcpy(&seg, ptr, sizeof(CurveSegment)); + animation.addCurveSegment(seg); + ptr += sizeof(CurveSegment); } // 🔹 Save using received filename @@ -891,92 +890,68 @@ 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); + // 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 motorKeyframes[NUM_CHANNELS]; - for (const auto& kf : anim.getKeyframes()) { - if (kf.motorId < NUM_CHANNELS) { - motorKeyframes[kf.motorId].push_back(kf); - } - } + // // Organize keyframes per motor + // std::vector 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; - }); - } + // // 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 (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 + // 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; - } - } + // // 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); - } + // // 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[0]->syncWritePos(ids, positions, NUM_CHANNELS); - nextFrameTime += frameDelay; - } + // positions[ch] = value; + // } + // Serial.println(positions[0]); + // servos[0]->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 - - uint32_t nextFrameTime = millis(); - - for (uint16_t frame = 0; frame < frameCount; frame++) { - // Wait until it's time for the next frame - while (millis() < nextFrameTime) { - // Optional: yield or do background tasks here - delay(1); - } - - // Send frame to servos - if (anim.getFramePositions(frame, positions)) { - servos[0]->syncWritePos(ids, positions, NUM_CHANNELS); - } - - // Schedule next frame - nextFrameTime += frameDelay; - } -} - void playLayeredAnimation(Animation& base, Animation& overlay) { uint16_t basePositions[NUM_CHANNELS]; uint16_t overlayPositions[NUM_CHANNELS]; diff --git a/animation.cpp b/animation.cpp index 99670a7..04aa5da 100644 --- a/animation.cpp +++ b/animation.cpp @@ -9,55 +9,56 @@ Animation::Animation() { memset(header.reserved, 0, sizeof(header.reserved)); } -void Animation::setFrame(uint16_t frameIndex, uint16_t channel, uint16_t value) { - if (frameIndex < MAX_FRAMES && channel < NUM_CHANNELS) { - data[frameIndex][channel] = value; +void Animation::addCurveSegment(const CurveSegment& segment) { + if (segment.motorID < NUM_CHANNELS) { + curves[segment.motorID].push_back(segment); } } -uint16_t Animation::getFrame(uint16_t frameIndex, uint16_t channel) const { - if (frameIndex < MAX_FRAMES && channel < NUM_CHANNELS) { - return data[frameIndex][channel]; - } - return 0; -} - -bool Animation::getFramePositions(uint16_t frameIndex, uint16_t* outPositions) { - if (frameIndex >= header.frameCount) return false; - - for (uint8_t ch = 0; ch < NUM_CHANNELS; ch++) { - outPositions[ch] = getFrame(frameIndex, ch); - } - return true; -} - -void Animation::addKeyframe(uint8_t motorId, uint16_t frame, uint16_t position) { - keyframes.push_back({ motorId, frame, position }); -} - -const std::vector& Animation::getKeyframes() const { - return keyframes; -} - -void Animation::printKeyframes() { - const std::vector& frames = getKeyframes(); - Serial.println("Keyframes:"); - for (size_t i = 0; i < frames.size(); ++i) { - const Keyframe& kf = frames[i]; - Serial.print(" ["); - Serial.print(i); - Serial.print("] Motor ID: "); - Serial.print(kf.motorId); - Serial.print(", Frame: "); - Serial.print(kf.frame); - Serial.print(", Position: "); - Serial.println(kf.position); +void Animation::clearCurves(uint8_t motorID) { + if (motorID < NUM_CHANNELS) { + curves[motorID].clear(); } } +void Animation::clearAllCurves() { + for (int i = 0; i < NUM_CHANNELS; ++i) { + curves[i].clear(); + } +} + +uint16_t Animation::getMotorPosition(uint8_t motorID, uint16_t timeCS) { + if (motorID >= NUM_CHANNELS) return 0; + + 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); + + 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; + + // Remap back to 0–4095 for PWM + return constrain((value + 1.0f) * 2047.5f, 0, 4095); + } + } + + return 2048; // Default center if no segment matches +} + + + void Animation::clear() { - memset(data, 0, sizeof(data)); - keyframes.clear(); + memset(data, 0, sizeof(data)); } uint16_t* Animation::getRawData() { @@ -83,7 +84,7 @@ bool Animation::saveToFile(const char* filename) { } } } - header.frameCount = lastFrame + 1; // +1 because frame index starts at 0 + header.frameCount = lastFrame + 1; File file = FFat.open(filename, FILE_WRITE); if (!file) return false; @@ -92,27 +93,26 @@ bool Animation::saveToFile(const char* filename) { file.write((uint8_t*)&header, sizeof(header)); file.write((uint8_t*)data, sizeof(data)); - // Write keyframe count - uint16_t keyframeCount = keyframes.size(); - file.write((uint8_t*)&keyframeCount, sizeof(keyframeCount)); - - // Write keyframes - for (const Keyframe& kf : keyframes) { - file.write(kf.motorId); - file.write(kf.frame & 0xFF); // low byte - file.write((kf.frame >> 8) & 0xFF); // high byte - - file.write(kf.position & 0xFF); - file.write((kf.position >> 8) & 0xFF); + // Count total curve segments + uint16_t curveCount = 0; + for (uint8_t ch = 0; ch < NUM_CHANNELS; ch++) { + curveCount += curves[ch].size(); } + // Write curve count + file.write((uint8_t*)&curveCount, sizeof(curveCount)); + // Write all curve segments + for (uint8_t ch = 0; ch < NUM_CHANNELS; ch++) { + for (const CurveSegment& seg : curves[ch]) { + file.write((uint8_t*)&seg, sizeof(CurveSegment)); + } + } file.close(); return true; } - bool Animation::loadFromFile(const char* filename) { File file = FFat.open(filename, FILE_READ); if (!file) return false; @@ -138,94 +138,30 @@ bool Animation::loadFromFile(const char* filename) { return false; } - - // Read keyframe count - uint16_t keyframeCount; - if (file.read((uint8_t*)&keyframeCount, sizeof(keyframeCount)) != sizeof(keyframeCount)) { + // Read curve count + uint16_t curveCount; + if (file.read((uint8_t*)&curveCount, sizeof(curveCount)) != sizeof(curveCount)) { file.close(); return false; } - // Read keyframes - keyframes.clear(); - for (uint16_t i = 0; i < keyframeCount; i++) { - Keyframe kf; - if (file.read(&kf.motorId, 1) != 1) { + // Clear existing curves + 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; } - uint8_t frameLow, frameHigh, posLow, posHigh; - if (file.read(&frameLow, 1) != 1 || file.read(&frameHigh, 1) != 1 || file.read(&posLow, 1) != 1 || file.read(&posHigh, 1) != 1) { - file.close(); - return false; + if (seg.motorID < NUM_CHANNELS) { + curves[seg.motorID].push_back(seg); } - - kf.frame = (frameHigh << 8) | frameLow; - kf.position = (posHigh << 8) | posLow; - - keyframes.push_back(kf); } - file.close(); return true; } - -void Animation::createSampleSweep(uint8_t seconds) { - clear(); - - const uint16_t sweepFrames = FRAMES_PER_SECOND * seconds; - const uint16_t totalFrames = sweepFrames * 2; // Up and down - - for (uint16_t frame = 0; frame < totalFrames; frame++) { - float progress; - if (frame < sweepFrames) { - // Sweep up: 0 → 1023 - progress = (float)frame / (sweepFrames - 1); - } else { - // Sweep down: 1023 → 0 - progress = 1.0f - ((float)(frame - sweepFrames) / (sweepFrames - 1)); - } - - uint16_t value = (uint16_t)(progress * 1023); - - for (uint8_t ch = 0; ch < NUM_CHANNELS; ch++) { - setFrame(frame, ch, value); - } - } - - header.frameCount = totalFrames; - - // 🧩 Add keyframes to match the sweep motion - for (uint8_t ch = 0; ch < NUM_CHANNELS; ch++) { - addKeyframe(ch, 0, 0); // Start at 0 - addKeyframe(ch, sweepFrames - 1, 1023); // Peak - addKeyframe(ch, totalFrames - 1, 0); // Return to 0 - } -} - -void Animation::createStaggeredSweep(uint8_t seconds) { - clear(); - - const uint16_t sweepFrames = FRAMES_PER_SECOND * seconds; - const uint16_t totalFrames = sweepFrames * 2; // Up and down - - for (uint16_t frame = 0; frame < totalFrames; frame++) { - float progress; - if (frame < sweepFrames) { - progress = (float)frame / (sweepFrames - 1); // 0 → 1 - } else { - progress = 1.0f - ((float)(frame - sweepFrames) / (sweepFrames - 1)); // 1 → 0 - } - - for (uint8_t ch = 0; ch < NUM_CHANNELS; ch++) { - float channelProgress = (ch % 2 == 0) ? progress : (1.0f - progress); - uint16_t value = (uint16_t)(channelProgress * 1023); - setFrame(frame, ch, value); - } - } - - header.frameCount = totalFrames; -} diff --git a/animation.h b/animation.h index cb00b2d..86bd49d 100644 --- a/animation.h +++ b/animation.h @@ -20,14 +20,20 @@ struct AnimationHeader { uint8_t reserved[8]; // 8–15 }; -struct __attribute__((packed)) Keyframe { - uint8_t motorId; - uint16_t frame; - uint16_t position; +struct __attribute__((packed)) CurveSegment { + uint8_t motorID; + uint16_t startTime; // centiseconds (0.01s) MAX 655.35 seconds + uint16_t endTime; + // 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 }; + class Animation { public: Animation(); @@ -36,9 +42,11 @@ public: uint16_t getFrame(uint16_t frameIndex, uint16_t channel) const; bool getFramePositions(uint16_t frameIndex, uint16_t* outPositions); - void addKeyframe(uint8_t motorId, uint16_t frame, uint16_t position); - const std::vector& getKeyframes() const; - void printKeyframes(); + void addCurveSegment(const CurveSegment& segment); + void clearCurves(uint8_t motorID); + void clearAllCurves(); + uint16_t getMotorPosition(uint8_t motorID, uint16_t timeCS); + void clear(); uint16_t* getRawData(); // Optional: for bulk access size_t getSize() const; @@ -51,7 +59,7 @@ public: private: uint16_t data[MAX_FRAMES][NUM_CHANNELS]; - std::vector keyframes; + std::vector curves[NUM_CHANNELS]; // One list per motor channel };