296 lines
7.3 KiB
C++
296 lines
7.3 KiB
C++
#include "animation.h"
|
|
|
|
Animation::Animation() {
|
|
clear();
|
|
memcpy(header.magic, "ANIM", 4);
|
|
header.version = 1;
|
|
header.frameRate = FRAMES_PER_SECOND;
|
|
header.frameCount = MAX_FRAMES;
|
|
memset(header.reserved, 0, sizeof(header.reserved));
|
|
}
|
|
|
|
void Animation::addCurveSegment(const CurveSegment& segment) {
|
|
curves[segment.motorID].push_back(segment);
|
|
}
|
|
|
|
|
|
void Animation::clearCurves(uint8_t motorID) {
|
|
curves.erase(motorID); // Completely remove the entry
|
|
}
|
|
|
|
void Animation::clearAllCurves() {
|
|
curves.clear(); // Wipe the entire map
|
|
}
|
|
|
|
|
|
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) {
|
|
// Convert uint16_t to float in range -1 to 1
|
|
auto toFloat = [](uint16_t v) {
|
|
return (float(v) / 65535.0f) * 2.0f - 1.0f;
|
|
};
|
|
|
|
// 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 * y0 + 3 * u * u * t * y1 + 3 * u * t * t * y2 + t * t * t * y3;
|
|
|
|
// Remap to PWM range
|
|
return constrain((value + 1.0f) * 2047.5f, 0, 4095);
|
|
}
|
|
}
|
|
|
|
return 2048; // Default center
|
|
}
|
|
|
|
|
|
|
|
|
|
void Animation::clear() {
|
|
//memset(data, 0, sizeof(data));
|
|
}
|
|
|
|
// uint16_t* Animation::getRawData() {
|
|
// return &data[0][0];
|
|
// }
|
|
|
|
// size_t Animation::getSize() const {
|
|
// return sizeof(data);
|
|
// }
|
|
|
|
void Animation::setFrameCount(uint16_t count) {
|
|
header.frameCount = count;
|
|
}
|
|
|
|
uint16_t Animation::getFrameCount() const {
|
|
return header.frameCount;
|
|
}
|
|
|
|
bool Animation::saveToFile(const char* filename) {
|
|
File file = FFat.open(filename, FILE_WRITE);
|
|
if (!file) return false;
|
|
|
|
// Write header
|
|
file.write((uint8_t*)&header, sizeof(header));
|
|
|
|
// Count total curve segments
|
|
uint16_t curveCount = 0;
|
|
for (const auto& [motorID, segments] : curves) {
|
|
curveCount += segments.size();
|
|
}
|
|
|
|
// Write curve count
|
|
file.write((uint8_t*)&curveCount, sizeof(curveCount));
|
|
|
|
// Write all curve segments
|
|
for (const auto& [motorID, segments] : curves) {
|
|
for (const CurveSegment& seg : segments) {
|
|
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;
|
|
|
|
// Read and validate header
|
|
AnimationHeader tempHeader;
|
|
if (file.read((uint8_t*)&tempHeader, sizeof(tempHeader)) != sizeof(tempHeader)) {
|
|
file.close();
|
|
return false;
|
|
}
|
|
|
|
if (strncmp(tempHeader.magic, "ANIM", 4) != 0 || tempHeader.version != 1) {
|
|
file.close();
|
|
return false;
|
|
}
|
|
|
|
header = tempHeader;
|
|
|
|
// Read curve count
|
|
uint16_t curveCount;
|
|
if (file.read((uint8_t*)&curveCount, sizeof(curveCount)) != sizeof(curveCount)) {
|
|
file.close();
|
|
return false;
|
|
}
|
|
|
|
// 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;
|
|
}
|
|
|
|
// Store segment directly — no motorID filtering
|
|
curves[seg.motorID].push_back(seg);
|
|
}
|
|
|
|
file.close();
|
|
return true;
|
|
}
|
|
|
|
|
|
void Animation::printCurves() {
|
|
Serial.println("PRINTING CURVES");
|
|
for (const auto& [motorID, segments] : curves) {
|
|
Serial.print("Motor ");
|
|
Serial.print(motorID);
|
|
Serial.print(": ");
|
|
Serial.println(segments.size());
|
|
|
|
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);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
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);
|
|
}
|