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

fixed flipping of motor position bytes during feedback

master
Jake 2025-11-02 15:22:19 +08:00
parent 21ddfee1cd
commit 4f376eaa1b
2 changed files with 32 additions and 101 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

129
HansonServo.ino
View File

@ -300,16 +300,13 @@ void handleCommand(uint8_t command, const uint8_t* payload, uint16_t length) {
void startPositionStream(const uint8_t* payload, uint16_t length) { void startPositionStream(const uint8_t* payload, uint16_t length) {
bool start = payload[0] != 0; bool start = payload[0] != 0;
servos[0]->disableTorque(10);
servos[0]->waitOnData1Byte(10); for (const Motor& motor : config.motors) {
servos[0]->disableTorque(11); servos[payload[0]]->setFeetechMode(motor.servoModel.major); // put feetech interface in correct mode
servos[0]->waitOnData1Byte(10); servos[0]->disableTorque(motor.motorID);
servos[0]->disableTorque(12);
servos[0]->waitOnData1Byte(10);
servos[0]->disableTorque(13);
servos[0]->waitOnData1Byte(10);
servos[0]->disableTorque(14);
servos[0]->waitOnData1Byte(10); servos[0]->waitOnData1Byte(10);
}
streamPositions = start; streamPositions = start;
} }
@ -961,7 +958,7 @@ void loop() {
if (millis() - lastSend > 50) { if (millis() - lastSend > 50) {
// Update config motor positions // Update config motor positions
for (const Motor& motor : config.motors) { for (const Motor& motor : config.motors) {
servos[payload[0]]->setFeetechMode(motor.servoModel.major); // put feetech interface in correct mode servos[0]->setFeetechMode(motor.servoModel.major); // put feetech interface in correct mode
uint16_t position = servos[0]->getPosition(motor.motorID); uint16_t position = servos[0]->getPosition(motor.motorID);
config.setMotorPosition(motor.motorID, position); config.setMotorPosition(motor.motorID, position);
//Serial.print(position); //Serial.print(position);
@ -969,6 +966,22 @@ void loop() {
} }
//Serial.println(); //Serial.println();
lastSend = millis(); lastSend = millis();
// for (const Motor& motor : config.motors) {
// uint16_t position = motor.position;
// uint8_t v1 = position >> 8;
// uint8_t v2 = position & 0xFF;
// Serial.print(v1);
// Serial.print("\t");
// Serial.print(v2);
// Serial.print(",\t");
// //Serial.print(position);
// //Serial.print(" ");
// //Serial.print((v1 << 8) | v2);
// }
// Serial.println();
} }
if (streamPositions) { if (streamPositions) {
@ -1082,9 +1095,15 @@ void SendMotorPositions() {
for (const Motor& motor : config.motors) { for (const Motor& motor : config.motors) {
uint16_t position = motor.position; uint16_t position = motor.position;
payload.push_back(position >> 8); // High byte
payload.push_back(position & 0xFF); // Low byte payload.push_back(position & 0xFF); // Low byte
payload.push_back(position >> 8); // High byte
//Serial.print(position);
//Serial.print("\t");
// Serial.print(position & 0xFF);
// Serial.print(",\t");
} }
//Serial.println();
sendMessage(payload.data(), payload.size(), POSITION_STREAM); sendMessage(payload.data(), payload.size(), POSITION_STREAM);
} }
@ -1183,94 +1202,6 @@ void playAnimation(Animation& animation) {
void playAnimationOLD(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);
// // 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;
// });
// }
// 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
// // 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);
// }
// positions[ch] = value;
// }
// Serial.println(positions[0]);
// servos[0]->syncWritePos(ids, positions, NUM_CHANNELS);
// nextFrameTime += frameDelay;
// }
}
// void playLayeredAnimation(Animation& base, Animation& overlay) {
// uint16_t basePositions[NUM_CHANNELS];
// uint16_t overlayPositions[NUM_CHANNELS];
// uint16_t finalPositions[NUM_CHANNELS];
// const uint16_t frameCount = base.getFrameCount();
// const uint32_t frameDelay = 1000 / FRAMES_PER_SECOND;
// uint32_t nextFrameTime = millis();
// for (uint16_t frame = 0; frame < frameCount; frame++) {
// while (millis() < nextFrameTime) delay(1);
// base.getFramePositions(frame, basePositions);
// overlay.getFramePositions(frame, overlayPositions);
// for (uint8_t ch = 0; ch < NUM_CHANNELS; ch++) {
// finalPositions[ch] = (basePositions[ch] + overlayPositions[ch]) / 2;
// }
// servos[0]->syncWritePos(ids, finalPositions, NUM_CHANNELS);
// nextFrameTime += frameDelay;
// }
// }
void SCSPingAll() { void SCSPingAll() {
std::vector<uint8_t> successfulAddresses; std::vector<uint8_t> successfulAddresses;
servos[0]->pingAll(successfulAddresses); servos[0]->pingAll(successfulAddresses);

2
robotconfig.cpp
View File

@ -73,7 +73,7 @@ String RobotConfig::serializeJSON() const {
output += "{"; output += "{";
output += "\"motorID\":" + String(m.motorID) + ","; output += "\"motorID\":" + String(m.motorID) + ",";
output += "\"name\":\"" + m.name + "\","; output += "\"name\":\"" + m.name + "\",";
output += "\"model\":" + String(m.servoModel.minor); output += "\"model\":" + String(m.servoModel.major);
output += "}"; output += "}";
if (i < motors.size() - 1) { if (i < motors.size() - 1) {