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

adapted for curves rather than keyframes (untested)

master
Jake 2025-10-07 14:26:49 +08:00
parent 13ae278505
commit 22cdb34063
3 changed files with 145 additions and 226 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

147
HansonServo.ino
View File

@ -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<Keyframe> 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<Keyframe> 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];

182
animation.cpp
View File

@ -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];
void Animation::clearCurves(uint8_t motorID) {
if (motorID < NUM_CHANNELS) {
curves[motorID].clear();
}
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);
void Animation::clearAllCurves() {
for (int i = 0; i < NUM_CHANNELS; ++i) {
curves[i].clear();
}
return true;
}
void Animation::addKeyframe(uint8_t motorId, uint16_t frame, uint16_t position) {
keyframes.push_back({ motorId, frame, position });
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);
}
}
const std::vector<Keyframe>& Animation::getKeyframes() const {
return keyframes;
return 2048; // Default center if no segment matches
}
void Animation::printKeyframes() {
const std::vector<Keyframe>& 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::clear() {
memset(data, 0, sizeof(data));
keyframes.clear();
}
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;
}

24
animation.h
View File

@ -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<Keyframe>& 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<Keyframe> keyframes;
std::vector<CurveSegment> curves[NUM_CHANNELS]; // One list per motor channel
};