id changes now work trhough interface
parent
e5d73fca5d
commit
21ddfee1cd
171
HansonServo.ino
171
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<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++) {
|
||||
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++;
|
||||
|
|
|
|||
29
feetech.cpp
29
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<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();
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
Loading…
Reference in New Issue