diff --git a/HansonServo.ino b/HansonServo.ino index 38994b8..0c8e6c4 100644 --- a/HansonServo.ino +++ b/HansonServo.ino @@ -58,6 +58,47 @@ uint8_t idsSTS[NUM_CHANNELS] = { 15, 103 }; uint16_t pos1STS[] = { 0, 0 }; uint16_t pos2STS[] = { 1023, 1023 }; +const uint8_t testPayload[] = { + // 🔹 Filename block (length = 17) + 17, 0, + '/', 'p', 'o', 'i', 'n', 't', 'c', 'u', 'r', 'v', 'e', '2', '.', 'a', 'n', 'i', 'm', + + // 🔹 Header: "ANIM", frameCount=800, version=1, frameRate=50, reserved[8] + 'A', 'N', 'I', 'M', + 32, 3, // frameCount = 800 (little-endian) + 1, // version + 50, // frameRate + 0, 0, 0, 0, 0, 0, 0, 0, // reserved + + // 🔹 Curve count = 2 + 2, 0, + + // 🔹 Curve Segment 1 + 10, // motorID + 0, 0, // startTime + 40, 0, // endTime + 44, 1, // startPointY = 300 + 10, 0, // startHandleX + 44, 1, // startHandleY = 300 + 30, 0, // endHandleX + 0, 0, // endHandleY + 0, 0, // endPointY + + // 🔹 Curve Segment 2 + 10, // motorID + 40, 0, // startTime + 32, 3, // endTime = 800 + 0, 0, // startPointY + 200, 0, // startHandleX + 0, 0, // startHandleY + 88, 2, // endHandleX = 600 + 44, 1, // endHandleY = 300 + 44, 1 // endPointY = 300 +}; + +const uint16_t testPayloadLength = sizeof(testPayload); + + void setup() { Serial.begin(1000000); for (int i = 0; i < 5; i++) { @@ -93,14 +134,19 @@ void setup() { } - + // anim.saveToFile("/scurve.anim"); // Serial.println("SAVED"); - //anim.loadFromFile("/scurve.anim"); + handleSaveFile(testPayload, testPayloadLength); + anim.loadFromFile("/pointcurve2.anim"); + Serial.println(anim.header.frameCount); + anim.printCurves(); + // Serial.print("CurveSegment size: "); + // Serial.println(sizeof(CurveSegment)); //anim.createBasicSCurve(); // Serial.println("loading"); - //anim.saveToFile("/pointcurve.anim"); - //playAnimation(anim); + //anim.saveToFile("/pointcurve.anim"); + //playAnimation(anim); // Serial.println("DONE"); // anim.createEaseOutCurve(); // playAnimation(anim); @@ -587,12 +633,16 @@ bool parseAnimationPayload(const uint8_t* payload, uint16_t length, Animation& a return false; } - animation.header.frameCount = (ptr[4] << 8) | ptr[5]; + animation.header.frameCount = ptr[5] << 8 | ptr[4]; // little-endian ✅ + animation.header.version = ptr[6]; animation.header.frameRate = ptr[7]; memcpy(animation.header.reserved, ptr + 8, 8); - uint16_t curveCount = ptr[16] | (ptr[17] << 8); + uint16_t curveCount = ptr[17] << 8 | ptr[16]; // little-endian ✅ + Serial.print("curveCount: "); + Serial.println(curveCount); + ptr += 18; if (length < (ptr - payload) + curveCount * sizeof(CurveSegment)) { @@ -606,6 +656,27 @@ bool parseAnimationPayload(const uint8_t* payload, uint16_t length, Animation& a for (uint16_t i = 0; i < curveCount; i++) { CurveSegment seg; memcpy(&seg, ptr, sizeof(CurveSegment)); + Serial.print("Segment "); + Serial.print(i); + Serial.print(": motorID="); + Serial.print(seg.motorID); + Serial.print(", startTime="); + Serial.print(seg.startTime); + Serial.print(", endTime="); + Serial.print(seg.endTime); + Serial.print(", startPointY="); + Serial.print(seg.startPointY); + Serial.print(", startHandleX="); + Serial.print(seg.startHandleX); + Serial.print(", startHandleY="); + Serial.print(seg.startHandleY); + Serial.print(", endHandleX="); + Serial.print(seg.endHandleX); + Serial.print(", endHandleY="); + Serial.print(seg.endHandleY); + Serial.print(", endPointY="); + Serial.println(seg.endPointY); + animation.addCurveSegment(seg); ptr += sizeof(CurveSegment); } @@ -907,7 +978,7 @@ void deleteFile(fs::FS& fs, const char* path) { void playAnimation(Animation& animation) { - uint16_t durationCS = animation.getFrameCount(); + 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; @@ -922,11 +993,11 @@ void playAnimation(Animation& animation) { 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); + uint16_t pos = animation.getMotorPosition(0, timeCS); + pos = pos / 4; + servos[0]->sendWritePos(10, pos); + + Serial.println(pos); //} nextFrameTime += frameIntervalMS; diff --git a/animation.cpp b/animation.cpp index af8ded7..caca1ff 100644 --- a/animation.cpp +++ b/animation.cpp @@ -10,23 +10,19 @@ Animation::Animation() { } void Animation::addCurveSegment(const CurveSegment& segment) { - if (segment.motorID < NUM_CHANNELS) { - curves[segment.motorID].push_back(segment); - } + curves[segment.motorID].push_back(segment); } + void Animation::clearCurves(uint8_t motorID) { - if (motorID < NUM_CHANNELS) { - curves[motorID].clear(); - } + curves.erase(motorID); // Completely remove the entry } void Animation::clearAllCurves() { - for (int i = 0; i < NUM_CHANNELS; ++i) { - curves[i].clear(); - } + curves.clear(); // Wipe the entire map } + uint16_t Animation::getMotorPosition(uint8_t motorID, uint16_t timeCS) { if (motorID >= NUM_CHANNELS) return 0; @@ -102,37 +98,24 @@ 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; - File file = FFat.open(filename, FILE_WRITE); if (!file) return false; - // Write header and motion data + // Write header file.write((uint8_t*)&header, sizeof(header)); - //file.write((uint8_t*)data, sizeof(data)); // Count total curve segments uint16_t curveCount = 0; - for (uint8_t ch = 0; ch < NUM_CHANNELS; ch++) { - curveCount += curves[ch].size(); + for (const auto& [motorID, segments] : curves) { + curveCount += segments.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]) { + for (const auto& [motorID, segments] : curves) { + for (const CurveSegment& seg : segments) { file.write((uint8_t*)&seg, sizeof(CurveSegment)); } } @@ -141,7 +124,6 @@ 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; @@ -160,13 +142,6 @@ 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; - // } - // Read curve count uint16_t curveCount; if (file.read((uint8_t*)&curveCount, sizeof(curveCount)) != sizeof(curveCount)) { @@ -185,9 +160,8 @@ bool Animation::loadFromFile(const char* filename) { return false; } - if (seg.motorID < NUM_CHANNELS) { - curves[seg.motorID].push_back(seg); - } + // Store segment directly — no motorID filtering + curves[seg.motorID].push_back(seg); } file.close(); @@ -196,55 +170,39 @@ bool Animation::loadFromFile(const char* filename) { 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.println("PRINTING CURVES"); + for (const auto& [motorID, segments] : curves) { Serial.print("Motor "); Serial.print(motorID); - Serial.println(":"); + Serial.print(": "); + Serial.println(segments.size()); - 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); + for (const auto& seg : segments) { + Serial.print(" Segment: "); + Serial.print("startTime="); + Serial.print(seg.startTime); + Serial.print(", endTime="); + Serial.print(seg.endTime); + Serial.print(", startPointY="); + Serial.print(seg.startPointY); + Serial.print(", startHandleX="); + Serial.print(seg.startHandleX); + Serial.print(", startHandleY="); + Serial.print(seg.startHandleY); + Serial.print(", endHandleX="); + Serial.print(seg.endHandleX); + Serial.print(", endHandleY="); + Serial.print(seg.endHandleY); + Serial.print(", endPointY="); + Serial.println(seg.endPointY); } } - - Serial.println("======================"); } + + void Animation::createBasicSCurve() { clearAllCurves(); diff --git a/animation.h b/animation.h index 48ad797..0796da8 100644 --- a/animation.h +++ b/animation.h @@ -4,7 +4,7 @@ #include #include "FS.h" #include "FFat.h" -#include +#include #define NUM_CHANNELS 5 @@ -63,7 +63,7 @@ public: private: //uint16_t data[MAX_FRAMES][NUM_CHANNELS]; - std::vector curves[NUM_CHANNELS]; // One list per motor channel + std::unordered_map> curves; };