From 4f376eaa1b01ec62922b34519f67de01d48d1a45 Mon Sep 17 00:00:00 2001 From: Jake Date: Sun, 2 Nov 2025 15:22:19 +0800 Subject: [PATCH] fixed flipping of motor position bytes during feedback --- HansonServo.ino | 131 ++++++++++++------------------------------------ robotconfig.cpp | 2 +- 2 files changed, 32 insertions(+), 101 deletions(-) diff --git a/HansonServo.ino b/HansonServo.ino index b977e9a..9f2fe0f 100644 --- a/HansonServo.ino +++ b/HansonServo.ino @@ -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) { bool start = payload[0] != 0; - servos[0]->disableTorque(10); - servos[0]->waitOnData1Byte(10); - servos[0]->disableTorque(11); - servos[0]->waitOnData1Byte(10); - servos[0]->disableTorque(12); - servos[0]->waitOnData1Byte(10); - servos[0]->disableTorque(13); - servos[0]->waitOnData1Byte(10); - servos[0]->disableTorque(14); - servos[0]->waitOnData1Byte(10); + + for (const Motor& motor : config.motors) { + servos[payload[0]]->setFeetechMode(motor.servoModel.major); // put feetech interface in correct mode + servos[0]->disableTorque(motor.motorID); + servos[0]->waitOnData1Byte(10); + } + streamPositions = start; } @@ -961,7 +958,7 @@ void loop() { if (millis() - lastSend > 50) { // Update config motor positions 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); config.setMotorPosition(motor.motorID, position); //Serial.print(position); @@ -969,6 +966,22 @@ void loop() { } //Serial.println(); 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) { @@ -1082,9 +1095,15 @@ void SendMotorPositions() { for (const Motor& motor : config.motors) { uint16_t position = motor.position; - payload.push_back(position >> 8); // High 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); } @@ -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 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() { std::vector successfulAddresses; servos[0]->pingAll(successfulAddresses); diff --git a/robotconfig.cpp b/robotconfig.cpp index c991003..716190c 100644 --- a/robotconfig.cpp +++ b/robotconfig.cpp @@ -73,7 +73,7 @@ String RobotConfig::serializeJSON() const { output += "{"; output += "\"motorID\":" + String(m.motorID) + ","; output += "\"name\":\"" + m.name + "\","; - output += "\"model\":" + String(m.servoModel.minor); + output += "\"model\":" + String(m.servoModel.major); output += "}"; if (i < motors.size() - 1) {