From 21ddfee1cdb20d5fc6c15d97b70efb77e5657e3a Mon Sep 17 00:00:00 2001 From: Jake Date: Sun, 2 Nov 2025 14:22:30 +0800 Subject: [PATCH] id changes now work trhough interface --- HansonServo.ino | 171 +++++++++++++----------------------------------- feetech.cpp | 29 ++++++-- feetech.h | 5 +- robotconfig.cpp | 5 +- 4 files changed, 76 insertions(+), 134 deletions(-) diff --git a/HansonServo.ino b/HansonServo.ino index 01dffa9..b977e9a 100644 --- a/HansonServo.ino +++ b/HansonServo.ino @@ -151,74 +151,12 @@ void setup() { } Serial.println(output); - //anim.saveToFile("/scurve.anim"); - //Serial.println("SAVED"); - //handleSaveFile(testPayload, testPayloadLength); - //Serial.println("Loading test.anim"); - //anim.loadFromFile("/test.anim"); - //Serial.println(anim.header.frameCount); - //anim.printCurves(); - // Serial.print("CurveSegment size: "); - // Serial.println(sizeof(CurveSegment)); - //anim.createBasicSCurve(); - // Serial.println("loading"); - //anim.saveToFile("/pointcurve.anim"); - //playAnimation(anim); - // Serial.println("DONE"); - // anim.createEaseOutCurve(); - // playAnimation(anim); - //anim.printCurves(); - // PrintFileList(); - // anim.loadFromFile("/bob.anim"); - // anim.printKeyframes(); - // playAnimation(anim); - // ClearFiles(); - // // PrintFileList(); - // sweep.clear(); - // sweep.createSampleSweep(4); - // sweep.saveToFile("/sweep.anim"); - // stagger.clear(); - // stagger.createStaggeredSweep(4); - // stagger.saveToFile("/stagger.anim"); - // //delay(9999); - // sweep.printKeyframes(); - - - // anim.clear(); - // if (!sweep.loadFromFile("/sweep.anim")) { - // Serial.println("Failed to load animation"); - // return; - // } else { - // Serial.println("Loaded sweep anim"); - // } - - // if (!stagger.loadFromFile("/stagger.anim")) { - // Serial.println("Failed to load animation"); - // return; - // } else { - // Serial.println("Loaded stagger anim"); - // } - - - // Serial.println(anim.getFrame(0, 0)); // Should show saved value - - //SetID(12, 16); - - //Serial.println(servos[1]->setLockSTS(16, 0)); - // Serial.println("Enable torque"); - // Serial.println(servos[0]->enableTorque(13)); - // Serial.println(servos[1]->enableTorque(13)); - // delay(3000); - // Serial.println("Disable torque"); - // Serial.println(servos[0]->disableTorque(13)); - // Serial.println(servos[1]->disableTorque(13)); - // delay(3000); - // servos[0]->setMinAngleLimit(13, 100); - // servos[1]->setMinAngleLimit(13, 900); - // servos[0]->setMaxAngleLimit(13, 500); - // servos[1]->setMaxAngleLimit(13, 3900); - //SetID(13, 17); + // PLAY ANIMATION + // anim.loadFromFile("/asdf.anim"); + // currentAnimation = &anim; + // currentAnimation->setActive(true); + // playMode = PLAY_ONCE; } void SetID(uint8_t oldID, uint8_t newID) { @@ -401,8 +339,8 @@ void handleConfigUpdate(const uint8_t* payload, uint16_t length) { if (offset + 5 > length) break; // MODEL(2) + ID(2) + nameLength(1) ServoModel model; - model.minor = payload[offset++]; model.major = payload[offset++]; + model.minor = payload[offset++]; uint16_t motorID = payload[offset++] | (payload[offset++] << 8); @@ -444,6 +382,18 @@ void handleDataWrite(const uint8_t* payload, uint16_t length) { uint8_t id = payload[1]; uint8_t writeByte = payload[2]; + uint8_t major = config.getMotorModel(id); + servos[payload[0]]->setFeetechMode(major); // put feetech interface in correct mode + + + // Special case for ID changes, perform unlock, change, lock routine + if (writeByte == 5) { + sendMessage("CHANGING ID"); + servos[channel]->changeID(id, payload[4]); + return; + } + + // payload[4] indicates if its a single byte, or 2 bytes of information if (payload[4] == 1) { servos[channel]->write1Byte(id, writeByte, payload[4]); uint8_t response = servos[channel]->waitOnData1Byte(10); @@ -827,6 +777,7 @@ void handleSetPosition(const uint8_t* payload, uint16_t length) { // Dynamically allocate arrays uint8_t ids[motorCount]; uint16_t positions[motorCount]; + uint16_t speeds[motorCount]; for (uint8_t i = 0; i < motorCount; ++i) { uint8_t motorId = payload[i * 3]; @@ -834,7 +785,7 @@ void handleSetPosition(const uint8_t* payload, uint16_t length) { ids[i] = motorId; positions[i] = position; - + speeds[i] = 0; // Optional: update internal state or debug // Serial.printf("Motor %d → %d\n", motorId, position); } @@ -843,13 +794,16 @@ void handleSetPosition(const uint8_t* payload, uint16_t length) { for (int i = 0; i < motorCount; i++) { if (config.getMotorModel(ids[i]) == 9) { positions[i] = flipBytes(positions[i]); + speeds[i] = map(speeds[i], 0, 4095, 0, 254); + speeds[i] = flipBytes(speeds[i]); } else { positions[i] = map(positions[i], 0, 4095, 0, 1023); + speeds[i] = map(speeds[i], 0, 4095, 0, 1000); } } // Send sync write to all motors - servos[0]->syncWritePos(ids, positions, motorCount); + servos[0]->syncWritePos(ids, positions, speeds, motorCount); String readablePayload = encodeMotorPositionsReadable(ids, positions, motorCount); sendMessage(readablePayload, CMD_MESSAGE); @@ -875,6 +829,7 @@ String encodeMotorPositionsReadable(const uint8_t* ids, const uint16_t* position + // Scans 0-254 and responds with the channel and ID as each successful ping is received // Signals end by responding with channel and 255 void handleScanChannel(const uint8_t* payload, uint16_t length) { @@ -894,6 +849,7 @@ void handleScanChannel(const uint8_t* payload, uint16_t length) { response[1] = i; // responding address uint16_t model = servos[payload[0]]->getModel(i); + servos[payload[0]]->setFeetechMode(model); // put feetech interface in correct mode uint16_t minAngleLimit = servos[payload[0]]->getMinAngleLimit(i); uint16_t maxAngleLimit = servos[payload[0]]->getMaxAngleLimit(i); uint16_t position = servos[payload[0]]->getPosition(i); @@ -995,67 +951,23 @@ void loop() { runNodeAnimation(); - // for (int i = 0; i < 1023; i++) { - // servos[0]->sendWritePos(10, i); - // Serial.println(servos[0]->waitOnData1Byte(10)); - // delay(20); - // } HandleSerialRequests(); - // for (int i = 0; i < 500; i+=32) { - // // Serial.print("WRITE: "); - // // Serial.print(i); - // // Serial.print(" "); - // // Serial.println(servos[1]->setMinAngleLimit(16, i)); - // Serial.print("READ: "); - // Serial.println(servos[1]->getMinAngleLimit(16)); - // delay(2000); - // } - // servos[0]->sendWritePos(13, 0); - // servos[1]->sendWritePos(13, 0); - // Serial.print(servos[0]->getMinAngleLimit(13)); - // Serial.print("\t"); - // Serial.print(servos[0]->getMaxAngleLimit(13)); - // Serial.print("\t"); - // Serial.print(servos[1]->getMinAngleLimit(13)); - // Serial.print("\t"); - // Serial.println(servos[1]->getMaxAngleLimit(13)); - // delay(1000); - // // - // for (int i = 0; i < 4095; i+=16){ - // Serial.println(i); - // servos[0]->sendWritePos(13, i); - // servos[1]->sendWritePos(13, i); - // delay(20); - // } - // delay(1000); - // for (int i = 4095; i > 0; i-=16){ - // Serial.println(i); - // servos[0]->sendWritePos(13, i); - // servos[1]->sendWritePos(13, i); - // delay(20); - // } - // delay(1000); - // delay(1000); - // for (int i = 0; i< 100; i++){ - // Serial.println(servos[1]->getGoalSpeed(13)); - // delay(10); - // } - // servos[0]->sendWritePos(13, 0); - // for (int i = 0; i< 100; i++){ - // Serial.println(servos[1]->getGoalSpeed(13)); - // delay(10); - // } + 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 uint16_t position = servos[0]->getPosition(motor.motorID); config.setMotorPosition(motor.motorID, position); + //Serial.print(position); + //Serial.print("\t"); } + //Serial.println(); lastSend = millis(); } @@ -1094,12 +1006,14 @@ void runNodeAnimation() { std::vector motorIDs; std::vector positions; + std::vector speeds; String message = String(currentTick) + String("\n"); for (const auto& [motorID, value] : outputs) { if (value != 65535) { motorIDs.push_back(motorID); positions.push_back(value); + speeds.push_back(0); config.setMotorPosition(motorID, value); if (config.setMotorEnabled(motorID, true)) { servos[0]->enableTorque(motorID); @@ -1123,17 +1037,20 @@ void runNodeAnimation() { } // FLIP BYTES FOR STS SERVOS -for (int i = 0; i < motorCount; i++) { - if (config.getMotorModel(motorIDs[i]) == 9) { - positions[i] = flipBytes(positions[i]); - } else { - positions[i] = map(positions[i], 0, 4095, 0, 1023); - } -} + for (int i = 0; i < motorCount; i++) { + if (config.getMotorModel(motorIDs[i]) == 9) { + positions[i] = flipBytes(positions[i]); + speeds[i] = map(speeds[i], 0, 4095, 0, 254); + speeds[i] = flipBytes(speeds[i]); + } else { + positions[i] = map(positions[i], 0, 4095, 0, 1023); + speeds[i] = map(speeds[i], 0, 4095, 0, 1000); + } + } // ✅ Send to servo controller - servos[0]->syncWritePos(motorIDs.data(), positions.data(), motorCount); + servos[0]->syncWritePos(motorIDs.data(), positions.data(), speeds.data(), motorCount); currentTick++; diff --git a/feetech.cpp b/feetech.cpp index 677756b..4ce9417 100644 --- a/feetech.cpp +++ b/feetech.cpp @@ -11,7 +11,7 @@ void Feetech::begin() { setModeReceive(); } -void Feetech::syncWritePos(uint8_t* ids, uint16_t* positions, uint8_t count) { +void Feetech::syncWritePos(uint8_t* ids, uint16_t* positions, uint16_t* speeds, uint8_t count) { const uint8_t DATA_LEN_PER_SERVO = 0x04; // 2 bytes pos + 2 bytes speed // Calculate packet length: instruction + address + data length + (count × data per servo) @@ -32,6 +32,7 @@ void Feetech::syncWritePos(uint8_t* ids, uint16_t* positions, uint8_t count) { // Servo data uint16_t pos; + uint16_t speed; for (uint8_t i = 0; i < count; i++) { packet[index++] = ids[i]; @@ -41,11 +42,12 @@ void Feetech::syncWritePos(uint8_t* ids, uint16_t* positions, uint8_t count) { // pos = positions[i]; // } pos = positions[i]; + speed = speeds[i]; - packet[index++] = (uint8_t)((pos >> 8) & 0xFF); // High byte - packet[index++] = (uint8_t)(pos & 0xFF); // Low byte - packet[index++] = 0x00; // Speed high byte - packet[index++] = 0x00; // Speed low byte + packet[index++] = (uint8_t)((pos >> 8) & 0xFF); // High byte + packet[index++] = (uint8_t)(pos & 0xFF); // Low byte + packet[index++] = (uint8_t)((speed >> 8) & 0xFF); // Speed high byte + packet[index++] = (uint8_t)(speed & 0xFF); // Speed low byte } // Checksum @@ -215,6 +217,13 @@ uint8_t Feetech::setID(uint8_t id, uint8_t newId) { return waitOnData1Byte(10); } +// Performs entire ID changing routine +uint8_t Feetech::changeID(uint8_t id, uint8_t newId) { + setLock(id, 0); + setID(id, newId); + setLock(newId, 1); +} + uint8_t Feetech::getBaudRate(uint8_t id) { sendRequest(id, REQUEST_BAUD_RATE, 1); return waitOnData1Byte(10); @@ -630,6 +639,16 @@ void Feetech::setFeetechMode(FeetechMode newMode) { feetechMode = newMode; } +// Pass motor model number (major) to set SCS/STS mode +void Feetech::setFeetechMode(uint8_t modelMajor) { + feetechMode = static_cast(modelMajor); +} + +void Feetech::setFeetechMode(uint16_t model) { + uint8_t major = model & 0xFF; // Get major model + setFeetechMode(major); +} + void Feetech::update() { if (Serial.available()) { setModeTransmit(); diff --git a/feetech.h b/feetech.h index 5fc3748..0127b94 100644 --- a/feetech.h +++ b/feetech.h @@ -26,6 +26,7 @@ public: uint16_t getModel(uint8_t id); uint8_t getID(uint8_t id); uint8_t setID(uint8_t id, uint8_t newId); + uint8_t changeID(uint8_t id, uint8_t newId); uint8_t getBaudRate(uint8_t id); uint16_t getMinAngleLimit(uint8_t id); uint16_t setMinAngleLimit(uint8_t id, uint16_t limit); @@ -55,7 +56,7 @@ public: uint8_t getVoltage(uint8_t id); void sendRequest(uint8_t id, uint8_t instruction, uint8_t byteCount); uint8_t sendWritePos(uint8_t id, uint16_t position); - void syncWritePos(uint8_t* ids, uint16_t* positions, uint8_t count); + void syncWritePos(uint8_t* ids, uint16_t* positions, uint16_t* speeds, uint8_t count); void write1Byte(uint8_t id, byte instruction, uint8_t data); void write2Bytes(uint8_t id, byte instruction, uint16_t data); void pingAll(std::vector& successfulAddresses); @@ -72,6 +73,8 @@ public: uint8_t enableTorque(uint8_t id); uint8_t disableTorque(uint8_t id); void setFeetechMode(FeetechMode newMode); + void setFeetechMode(uint8_t majorModel); + void setFeetechMode(uint16_t model); static const byte PING = 0x01; // QUERY THE WORKING STATUS static const byte READ_DATA = 0x02; // READ DATA diff --git a/robotconfig.cpp b/robotconfig.cpp index 8ac6288..c991003 100644 --- a/robotconfig.cpp +++ b/robotconfig.cpp @@ -14,7 +14,7 @@ uint16_t RobotConfig::getMotorPosition(uint8_t motorID) const { uint8_t RobotConfig::getMotorModel(uint8_t motorID) { for (const Motor& motor : motors) { if (motor.motorID == motorID) { - return motor.servoModel.minor; + return motor.servoModel.major; } } // Return 0 if motor not found @@ -116,6 +116,9 @@ std::vector RobotConfig::serializeToBytes() const { buffer.push_back(m.name[i]); } + buffer.push_back(m.servoModel.major); + buffer.push_back(m.servoModel.minor); + // position (uint16_t → 2 bytes) buffer.push_back((m.position >> 8) & 0xFF); buffer.push_back(m.position & 0xFF);