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

second channel in motor panel works properly now

master
Jake 2025-11-02 18:41:56 +08:00
parent 8ad8589cf5
commit 6d80ad31e6
3 changed files with 26 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
HansonServo.ino
View File

@ -107,7 +107,7 @@ void setup() {
servos[0] = new Feetech(Serial1, DE_PIN, RE_PIN, CH0_TX_PIN, CH0_RX_PIN); // SCS
servos[0]->setFeetechMode(Feetech::MODE_SCS);
servos[1] = new Feetech(Serial2, DE_PIN, RE_PIN, CH1_TX_PIN, CH1_RX_PIN); // STS
servos[1]->setFeetechMode(Feetech::MODE_STS);
servos[1]->setFeetechMode(Feetech::MODE_SCS);
// pinMode(RX_PIN, OUTPUT);
// pinMode(TX_PIN, OUTPUT);
@ -153,12 +153,20 @@ void setup() {
// PLAY ANIMATION
anim.loadFromFile("/aa.anim");
currentAnimation = &anim;
currentAnimation->setActive(true);
playMode = PLAY_ONCE;
// anim.loadFromFile("/aa.anim");
// currentAnimation = &anim;
// currentAnimation->setActive(true);
// playMode = PLAY_ONCE;
//Serial.println(anim.header.frameCount);
//Serial.println(anim.printCurves());
// Serial.println("Model");
// Serial.println(servos[0]->getModel(10));
// delay(20);
// Serial.println(servos[0]->getModel(11));
// delay(50);
// Serial.println(servos[0]->getModel(12));
// Serial.println(servos[0]->getModel(17));
// Serial.println(servos[0]->getModel(14));
}
void SetID(uint8_t oldID, uint8_t newID) {
@ -381,8 +389,9 @@ 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
uint8_t model = servos[payload[0]]->getMajorModel(id);// config.getMotorModel(id);
sendMessage(String(model));
servos[payload[0]]->setFeetechMode(model); // put feetech interface in correct mode
// Special case for ID changes, perform unlock, change, lock routine

8
feetech.cpp
View File

@ -204,7 +204,13 @@ uint8_t Feetech::disableTorque(uint8_t id) {
uint16_t Feetech::getModel(uint8_t id) {
sendRequest(id, REQUEST_MODEL, 2);
return waitOnData2Bytes(10);
return waitOnData2Bytes(50);
}
uint16_t Feetech::getMajorModel(uint8_t id) {
sendRequest(id, REQUEST_MODEL, 2);
uint16_t fullModel = waitOnData2Bytes(10);
return fullModel; // return low byte (major model)
}
uint8_t Feetech::getID(uint8_t id) {

4
feetech.h
View File

@ -24,6 +24,7 @@ public:
void sendPing(uint8_t id);
void clearEcho(uint8_t length);
uint16_t getModel(uint8_t id);
uint16_t getMajorModel(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);
@ -115,7 +116,8 @@ public:
static const byte REQUEST_VOLTAGE = 0x3E; // 1 byte
static const byte REQUEST_TEMPERATURE = 0x3F; // 1 byte
static const byte REQUEST_TEMPERATURE = 0x3F; // 1 byte
static const byte REQUEST_STATUS = 0x41; // 1 byte Bit0, Bit1, Bit2, Bit3, Bit4, and Bit5 are set to 1 to indicate the occurrence of a corresponding error (no voltage, no temperature, no overload). A corresponding bit of 0 indicates no error.
static const byte REQUEST_MOVING = 0x42; // 1 byte
static const byte REQUEST_CURRENT_CURRENT = 0x45; // 2 bytes