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]; animation.header.frameRate = ptr[7];
memcpy(animation.header.reserved, ptr + 8, 8); 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; ptr += 18;
if (length < (ptr - payload) + keyframeCount * 5) { if (length < (ptr - payload) + curveCount * sizeof(CurveSegment)) {
Serial.println("Payload too short for keyframes"); Serial.println("Payload too short for curve segments");
sendMessage("Payload too short"); sendMessage("Payload too short");
return false; return false;
} }
// 🔹 Parse keyframes // 🔹 Parse curve segments
animation.clear(); animation.clearAllCurves();
for (uint16_t i = 0; i < keyframeCount; i++) { for (uint16_t i = 0; i < curveCount; i++) {
uint8_t motorId = ptr[0]; CurveSegment seg;
uint16_t frame = ptr[1] | (ptr[2] << 8); memcpy(&seg, ptr, sizeof(CurveSegment));
uint16_t position = ptr[3] | (ptr[4] << 8); animation.addCurveSegment(seg);
animation.addKeyframe(motorId, frame, position); ptr += sizeof(CurveSegment);
ptr += 5;
} }
// 🔹 Save using received filename // 🔹 Save using received filename
@ -891,92 +890,68 @@ void deleteFile(fs::FS& fs, const char* path) {
} }
void playAnimation(Animation& anim) { void playAnimation(Animation& anim) {
uint16_t positions[NUM_CHANNELS]; // uint16_t positions[NUM_CHANNELS];
const uint16_t frameCount = 400; //anim.getFrameCount(); // const uint16_t frameCount = 400; //anim.getFrameCount();
const uint32_t frameDelay = 1000 / FRAMES_PER_SECOND; // const uint32_t frameDelay = 1000 / FRAMES_PER_SECOND;
uint32_t nextFrameTime = millis(); // uint32_t nextFrameTime = millis();
Serial.print("Frame Count: "); // Serial.print("Frame Count: ");
Serial.println(frameCount); // Serial.println(frameCount);
// Organize keyframes per motor // // Organize keyframes per motor
std::vector<Keyframe> motorKeyframes[NUM_CHANNELS]; // std::vector<Keyframe> motorKeyframes[NUM_CHANNELS];
for (const auto& kf : anim.getKeyframes()) { // for (const auto& kf : anim.getKeyframes()) {
if (kf.motorId < NUM_CHANNELS) { // if (kf.motorId < NUM_CHANNELS) {
motorKeyframes[kf.motorId].push_back(kf); // motorKeyframes[kf.motorId].push_back(kf);
} // }
} // }
// Sort keyframes per motor by frame // // Sort keyframes per motor by frame
for (int ch = 0; ch < NUM_CHANNELS; ch++) { // for (int ch = 0; ch < NUM_CHANNELS; ch++) {
std::sort(motorKeyframes[ch].begin(), motorKeyframes[ch].end(), // std::sort(motorKeyframes[ch].begin(), motorKeyframes[ch].end(),
[](const Keyframe& a, const Keyframe& b) { // [](const Keyframe& a, const Keyframe& b) {
return a.frame < b.frame; // return a.frame < b.frame;
}); // });
} // }
for (uint16_t frame = 0; frame < frameCount; frame++) { // for (uint16_t frame = 0; frame < frameCount; frame++) {
while (millis() < nextFrameTime) { // while (millis() < nextFrameTime) {
delay(1); // delay(1);
} // }
for (int ch = 0; ch < NUM_CHANNELS; ch++) { // for (int ch = 0; ch < NUM_CHANNELS; ch++) {
const auto& kfs = motorKeyframes[ch]; // const auto& kfs = motorKeyframes[ch];
uint16_t value = 512; // default position // uint16_t value = 512; // default position
// Find surrounding keyframes // // Find surrounding keyframes
Keyframe prev = { ch, 0, 512 }, next = { ch, frameCount, 512 }; // Keyframe prev = { ch, 0, 512 }, next = { ch, frameCount, 512 };
for (size_t i = 0; i < kfs.size(); i++) { // for (size_t i = 0; i < kfs.size(); i++) {
if (kfs[i].frame <= frame) { // if (kfs[i].frame <= frame) {
prev = kfs[i]; // prev = kfs[i];
} // }
if (kfs[i].frame > frame) { // if (kfs[i].frame > frame) {
next = kfs[i]; // next = kfs[i];
break; // break;
} // }
} // }
// Interpolate // // Interpolate
if (prev.frame == next.frame) { // if (prev.frame == next.frame) {
value = prev.position; // value = prev.position;
} else { // } else {
float t = float(frame - prev.frame) / (next.frame - prev.frame); // float t = float(frame - prev.frame) / (next.frame - prev.frame);
value = prev.position + t * (next.position - prev.position); // value = prev.position + t * (next.position - prev.position);
} // }
positions[ch] = value; // positions[ch] = value;
} // }
Serial.println(positions[0]); // Serial.println(positions[0]);
servos[0]->syncWritePos(ids, positions, NUM_CHANNELS); // servos[0]->syncWritePos(ids, positions, NUM_CHANNELS);
nextFrameTime += frameDelay; // 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) { void playLayeredAnimation(Animation& base, Animation& overlay) {
uint16_t basePositions[NUM_CHANNELS]; uint16_t basePositions[NUM_CHANNELS];
uint16_t overlayPositions[NUM_CHANNELS]; uint16_t overlayPositions[NUM_CHANNELS];

198
animation.cpp
View File

@ -9,55 +9,56 @@ Animation::Animation() {
memset(header.reserved, 0, sizeof(header.reserved)); memset(header.reserved, 0, sizeof(header.reserved));
} }
void Animation::setFrame(uint16_t frameIndex, uint16_t channel, uint16_t value) { void Animation::addCurveSegment(const CurveSegment& segment) {
if (frameIndex < MAX_FRAMES && channel < NUM_CHANNELS) { if (segment.motorID < NUM_CHANNELS) {
data[frameIndex][channel] = value; curves[segment.motorID].push_back(segment);
} }
} }
uint16_t Animation::getFrame(uint16_t frameIndex, uint16_t channel) const { void Animation::clearCurves(uint8_t motorID) {
if (frameIndex < MAX_FRAMES && channel < NUM_CHANNELS) { if (motorID < NUM_CHANNELS) {
return data[frameIndex][channel]; 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);
}
return true;
}
void Animation::addKeyframe(uint8_t motorId, uint16_t frame, uint16_t position) {
keyframes.push_back({ motorId, frame, position });
}
const std::vector<Keyframe>& Animation::getKeyframes() const {
return keyframes;
}
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::clearAllCurves() {
for (int i = 0; i < NUM_CHANNELS; ++i) {
curves[i].clear();
}
}
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);
}
}
return 2048; // Default center if no segment matches
}
void Animation::clear() { void Animation::clear() {
memset(data, 0, sizeof(data)); memset(data, 0, sizeof(data));
keyframes.clear();
} }
uint16_t* Animation::getRawData() { 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); File file = FFat.open(filename, FILE_WRITE);
if (!file) return false; 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*)&header, sizeof(header));
file.write((uint8_t*)data, sizeof(data)); file.write((uint8_t*)data, sizeof(data));
// Write keyframe count // Count total curve segments
uint16_t keyframeCount = keyframes.size(); uint16_t curveCount = 0;
file.write((uint8_t*)&keyframeCount, sizeof(keyframeCount)); for (uint8_t ch = 0; ch < NUM_CHANNELS; ch++) {
curveCount += curves[ch].size();
// 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);
} }
// 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(); file.close();
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;
@ -138,94 +138,30 @@ bool Animation::loadFromFile(const char* filename) {
return false; return false;
} }
// Read curve count
// Read keyframe count uint16_t curveCount;
uint16_t keyframeCount; if (file.read((uint8_t*)&curveCount, sizeof(curveCount)) != sizeof(curveCount)) {
if (file.read((uint8_t*)&keyframeCount, sizeof(keyframeCount)) != sizeof(keyframeCount)) {
file.close(); file.close();
return false; return false;
} }
// Read keyframes // Clear existing curves
keyframes.clear(); clearAllCurves();
for (uint16_t i = 0; i < keyframeCount; i++) {
Keyframe kf; // Read curve segments
if (file.read(&kf.motorId, 1) != 1) { for (uint16_t i = 0; i < curveCount; i++) {
CurveSegment seg;
if (file.read((uint8_t*)&seg, sizeof(CurveSegment)) != sizeof(CurveSegment)) {
file.close(); file.close();
return false; return false;
} }
uint8_t frameLow, frameHigh, posLow, posHigh; if (seg.motorID < NUM_CHANNELS) {
if (file.read(&frameLow, 1) != 1 || file.read(&frameHigh, 1) != 1 || file.read(&posLow, 1) != 1 || file.read(&posHigh, 1) != 1) { curves[seg.motorID].push_back(seg);
file.close();
return false;
} }
kf.frame = (frameHigh << 8) | frameLow;
kf.position = (posHigh << 8) | posLow;
keyframes.push_back(kf);
} }
file.close(); file.close();
return true; 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 uint8_t reserved[8]; // 8–15
}; };
struct __attribute__((packed)) Keyframe { struct __attribute__((packed)) CurveSegment {
uint8_t motorId; uint8_t motorID;
uint16_t frame; uint16_t startTime; // centiseconds (0.01s) MAX 655.35 seconds
uint16_t position; 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 { class Animation {
public: public:
Animation(); Animation();
@ -36,9 +42,11 @@ public:
uint16_t getFrame(uint16_t frameIndex, uint16_t channel) const; uint16_t getFrame(uint16_t frameIndex, uint16_t channel) const;
bool getFramePositions(uint16_t frameIndex, uint16_t* outPositions); bool getFramePositions(uint16_t frameIndex, uint16_t* outPositions);
void addKeyframe(uint8_t motorId, uint16_t frame, uint16_t position); void addCurveSegment(const CurveSegment& segment);
const std::vector<Keyframe>& getKeyframes() const; void clearCurves(uint8_t motorID);
void printKeyframes(); void clearAllCurves();
uint16_t getMotorPosition(uint8_t motorID, uint16_t timeCS);
void clear(); void clear();
uint16_t* getRawData(); // Optional: for bulk access uint16_t* getRawData(); // Optional: for bulk access
size_t getSize() const; size_t getSize() const;
@ -51,7 +59,7 @@ public:
private: private:
uint16_t data[MAX_FRAMES][NUM_CHANNELS]; uint16_t data[MAX_FRAMES][NUM_CHANNELS];
std::vector<Keyframe> keyframes; std::vector<CurveSegment> curves[NUM_CHANNELS]; // One list per motor channel
}; };