realrobotshk
/
HansonServo
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

curve animation save/load implemented (needs troubleshooting, y axis flattening somewhere

master
Jake 2025-10-09 00:04:29 +08:00
parent 06883652d0
commit 88fec6ef3c
3 changed files with 121 additions and 92 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

91
HansonServo.ino
View File

@ -58,6 +58,47 @@ uint8_t idsSTS[NUM_CHANNELS] = { 15, 103 };
uint16_t pos1STS[] = { 0, 0 }; uint16_t pos1STS[] = { 0, 0 };
uint16_t pos2STS[] = { 1023, 1023 }; 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() { void setup() {
Serial.begin(1000000); Serial.begin(1000000);
for (int i = 0; i < 5; i++) { for (int i = 0; i < 5; i++) {
@ -96,11 +137,16 @@ void setup() {
// anim.saveToFile("/scurve.anim"); // anim.saveToFile("/scurve.anim");
// Serial.println("SAVED"); // 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(); //anim.createBasicSCurve();
// Serial.println("loading"); // Serial.println("loading");
//anim.saveToFile("/pointcurve.anim"); //anim.saveToFile("/pointcurve.anim");
//playAnimation(anim); //playAnimation(anim);
// Serial.println("DONE"); // Serial.println("DONE");
// anim.createEaseOutCurve(); // anim.createEaseOutCurve();
// playAnimation(anim); // playAnimation(anim);
@ -587,12 +633,16 @@ bool parseAnimationPayload(const uint8_t* payload, uint16_t length, Animation& a
return false; 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.version = ptr[6];
animation.header.frameRate = ptr[7]; animation.header.frameRate = ptr[7];
memcpy(animation.header.reserved, ptr + 8, 8); 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; ptr += 18;
if (length < (ptr - payload) + curveCount * sizeof(CurveSegment)) { 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++) { for (uint16_t i = 0; i < curveCount; i++) {
CurveSegment seg; CurveSegment seg;
memcpy(&seg, ptr, sizeof(CurveSegment)); 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); animation.addCurveSegment(seg);
ptr += sizeof(CurveSegment); ptr += sizeof(CurveSegment);
} }
@ -907,7 +978,7 @@ void deleteFile(fs::FS& fs, const char* path) {
void playAnimation(Animation& animation) { void playAnimation(Animation& animation) {
uint16_t durationCS = animation.getFrameCount(); uint16_t durationCS = animation.getFrameCount();
const uint8_t fps = 48; const uint8_t fps = 48;
const uint32_t frameIntervalMS = 1000 / fps; // ~20.83 ms const uint32_t frameIntervalMS = 1000 / fps; // ~20.83 ms
const uint32_t totalDurationMS = durationCS * 10; const uint32_t totalDurationMS = durationCS * 10;
@ -922,11 +993,11 @@ void playAnimation(Animation& animation) {
uint16_t timeCS = (currentTime - startTime) / 10; uint16_t timeCS = (currentTime - startTime) / 10;
//for (uint8_t motorID = 0; motorID < NUM_CHANNELS; motorID++) { //for (uint8_t motorID = 0; motorID < NUM_CHANNELS; motorID++) {
uint16_t pos = animation.getMotorPosition(0, timeCS); uint16_t pos = animation.getMotorPosition(0, timeCS);
pos = pos / 4; pos = pos / 4;
servos[0]->sendWritePos(10, pos); servos[0]->sendWritePos(10, pos);
Serial.println(pos); Serial.println(pos);
//} //}
nextFrameTime += frameIntervalMS; nextFrameTime += frameIntervalMS;

114
animation.cpp
View File

@ -10,23 +10,19 @@ Animation::Animation() {
} }
void Animation::addCurveSegment(const CurveSegment& segment) { 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) { void Animation::clearCurves(uint8_t motorID) {
if (motorID < NUM_CHANNELS) { curves.erase(motorID); // Completely remove the entry
curves[motorID].clear();
}
} }
void Animation::clearAllCurves() { void Animation::clearAllCurves() {
for (int i = 0; i < NUM_CHANNELS; ++i) { curves.clear(); // Wipe the entire map
curves[i].clear();
}
} }
uint16_t Animation::getMotorPosition(uint8_t motorID, uint16_t timeCS) { uint16_t Animation::getMotorPosition(uint8_t motorID, uint16_t timeCS) {
if (motorID >= NUM_CHANNELS) return 0; if (motorID >= NUM_CHANNELS) return 0;
@ -102,37 +98,24 @@ uint16_t Animation::getFrameCount() const {
} }
bool Animation::saveToFile(const char* filename) { 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); File file = FFat.open(filename, FILE_WRITE);
if (!file) return false; if (!file) return false;
// Write header and motion data // Write header
file.write((uint8_t*)&header, sizeof(header)); file.write((uint8_t*)&header, sizeof(header));
//file.write((uint8_t*)data, sizeof(data));
// Count total curve segments // Count total curve segments
uint16_t curveCount = 0; uint16_t curveCount = 0;
for (uint8_t ch = 0; ch < NUM_CHANNELS; ch++) { for (const auto& [motorID, segments] : curves) {
curveCount += curves[ch].size(); curveCount += segments.size();
} }
// Write curve count // Write curve count
file.write((uint8_t*)&curveCount, sizeof(curveCount)); file.write((uint8_t*)&curveCount, sizeof(curveCount));
// Write all curve segments // Write all curve segments
for (uint8_t ch = 0; ch < NUM_CHANNELS; ch++) { for (const auto& [motorID, segments] : curves) {
for (const CurveSegment& seg : curves[ch]) { for (const CurveSegment& seg : segments) {
file.write((uint8_t*)&seg, sizeof(CurveSegment)); file.write((uint8_t*)&seg, sizeof(CurveSegment));
} }
} }
@ -141,7 +124,6 @@ bool Animation::saveToFile(const char* filename) {
return true; return true;
} }
bool Animation::loadFromFile(const char* filename) { bool Animation::loadFromFile(const char* filename) {
File file = FFat.open(filename, FILE_READ); File file = FFat.open(filename, FILE_READ);
if (!file) return false; if (!file) return false;
@ -160,13 +142,6 @@ bool Animation::loadFromFile(const char* filename) {
header = tempHeader; 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 // Read curve count
uint16_t curveCount; uint16_t curveCount;
if (file.read((uint8_t*)&curveCount, sizeof(curveCount)) != sizeof(curveCount)) { if (file.read((uint8_t*)&curveCount, sizeof(curveCount)) != sizeof(curveCount)) {
@ -185,9 +160,8 @@ bool Animation::loadFromFile(const char* filename) {
return false; return false;
} }
if (seg.motorID < NUM_CHANNELS) { // Store segment directly — no motorID filtering
curves[seg.motorID].push_back(seg); curves[seg.motorID].push_back(seg);
}
} }
file.close(); file.close();
@ -196,55 +170,39 @@ bool Animation::loadFromFile(const char* filename) {
void Animation::printCurves() { void Animation::printCurves() {
Serial.println("=== Curve Segments ==="); Serial.println("PRINTING CURVES");
for (const auto& [motorID, segments] : curves) {
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("Motor ");
Serial.print(motorID); Serial.print(motorID);
Serial.println(":"); Serial.print(": ");
Serial.println(segments.size());
for (size_t i = 0; i < curves[motorID].size(); ++i) { for (const auto& seg : segments) {
const CurveSegment& seg = curves[motorID][i]; Serial.print(" Segment: ");
Serial.print("startTime=");
Serial.print(" Segment "); Serial.print(seg.startTime);
Serial.print(i); Serial.print(", endTime=");
Serial.print(" | Time: "); Serial.print(seg.endTime);
Serial.print(seg.startTime * 0.01f, 2); Serial.print(", startPointY=");
Serial.print("s → "); Serial.print(seg.startPointY);
Serial.print(seg.endTime * 0.01f, 2); Serial.print(", startHandleX=");
Serial.println("s"); Serial.print(seg.startHandleX);
Serial.print(", startHandleY=");
Serial.print(" Start Point Y: "); Serial.print(seg.startHandleY);
Serial.println(toFloat(seg.startPointY), 3); Serial.print(", endHandleX=");
Serial.print(seg.endHandleX);
Serial.print(" Start Handle: ("); Serial.print(", endHandleY=");
Serial.print(seg.startHandleX * 0.01f, 2); Serial.print(seg.endHandleY);
Serial.print("s, "); Serial.print(", endPointY=");
Serial.print(toFloat(seg.startHandleY), 3); Serial.println(seg.endPointY);
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() { void Animation::createBasicSCurve() {
clearAllCurves(); clearAllCurves();

4
animation.h
View File

@ -4,7 +4,7 @@
#include <Arduino.h> #include <Arduino.h>
#include "FS.h" #include "FS.h"
#include "FFat.h" #include "FFat.h"
#include <vector> #include <unordered_map>
#define NUM_CHANNELS 5 #define NUM_CHANNELS 5
@ -63,7 +63,7 @@ public:
private: 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 std::unordered_map<uint8_t, std::vector<CurveSegment>> curves;
}; };