curve animation save/load implemented (needs troubleshooting, y axis flattening somewhere
parent
06883652d0
commit
88fec6ef3c
|
|
@ -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++) {
|
||||
|
|
@ -96,7 +137,12 @@ 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");
|
||||
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
110
animation.cpp
110
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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
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,10 +160,9 @@ bool Animation::loadFromFile(const char* filename) {
|
|||
return false;
|
||||
}
|
||||
|
||||
if (seg.motorID < NUM_CHANNELS) {
|
||||
// Store segment directly — no motorID filtering
|
||||
curves[seg.motorID].push_back(seg);
|
||||
}
|
||||
}
|
||||
|
||||
file.close();
|
||||
return true;
|
||||
|
|
@ -196,52 +170,36 @@ 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("======================");
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@
|
|||
#include <Arduino.h>
|
||||
#include "FS.h"
|
||||
#include "FFat.h"
|
||||
#include <vector>
|
||||
#include <unordered_map>
|
||||
|
||||
|
||||
#define NUM_CHANNELS 5
|
||||
|
|
@ -63,7 +63,7 @@ public:
|
|||
|
||||
private:
|
||||
//uint16_t data[MAX_FRAMES][NUM_CHANNELS];
|
||||
std::vector<CurveSegment> curves[NUM_CHANNELS]; // One list per motor channel
|
||||
std::unordered_map<uint8_t, std::vector<CurveSegment>> curves;
|
||||
|
||||
};
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue