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

id changes now work trhough interface

master
Jake 2025-11-02 14:22:30 +08:00
parent e5d73fca5d
commit 21ddfee1cd
4 changed files with 76 additions and 134 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

161
HansonServo.ino
View File

@ -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<uint8_t> motorIDs;
std::vector<uint16_t> positions;
std::vector<uint16_t> 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++) {
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++;

25
feetech.cpp
View File

@ -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)((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<FeetechMode>(modelMajor);
}
void Feetech::setFeetechMode(uint16_t model) {
uint8_t major = model & 0xFF; // Get major model
setFeetechMode(major);
}
void Feetech::update() {
if (Serial.available()) {
setModeTransmit();

5
feetech.h
View File

@ -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<uint8_t>& 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

5
robotconfig.cpp
View File

@ -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<uint8_t> 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);