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] = new Feetech(Serial1, DE_PIN, RE_PIN, CH0_TX_PIN, CH0_RX_PIN); // SCS
servos[0]->setFeetechMode(Feetech::MODE_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] = 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(RX_PIN, OUTPUT);
// pinMode(TX_PIN, OUTPUT); // pinMode(TX_PIN, OUTPUT);
@ -153,12 +153,20 @@ void setup() {
// PLAY ANIMATION // PLAY ANIMATION
anim.loadFromFile("/aa.anim"); // anim.loadFromFile("/aa.anim");
currentAnimation = &anim; // currentAnimation = &anim;
currentAnimation->setActive(true); // currentAnimation->setActive(true);
playMode = PLAY_ONCE; // playMode = PLAY_ONCE;
//Serial.println(anim.header.frameCount); //Serial.println(anim.header.frameCount);
//Serial.println(anim.printCurves()); //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) { 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 id = payload[1];
uint8_t writeByte = payload[2]; uint8_t writeByte = payload[2];
uint8_t major = config.getMotorModel(id); uint8_t model = servos[payload[0]]->getMajorModel(id);// config.getMotorModel(id);
servos[payload[0]]->setFeetechMode(major); // put feetech interface in correct mode sendMessage(String(model));
servos[payload[0]]->setFeetechMode(model); // put feetech interface in correct mode
// Special case for ID changes, perform unlock, change, lock routine // 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) { uint16_t Feetech::getModel(uint8_t id) {
sendRequest(id, REQUEST_MODEL, 2); 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) { uint8_t Feetech::getID(uint8_t id) {

2
feetech.h
View File

@ -24,6 +24,7 @@ public:
void sendPing(uint8_t id); void sendPing(uint8_t id);
void clearEcho(uint8_t length); void clearEcho(uint8_t length);
uint16_t getModel(uint8_t id); uint16_t getModel(uint8_t id);
uint16_t getMajorModel(uint8_t id);
uint8_t getID(uint8_t id); uint8_t getID(uint8_t id);
uint8_t setID(uint8_t id, uint8_t newId); uint8_t setID(uint8_t id, uint8_t newId);
uint8_t changeID(uint8_t id, uint8_t newId); uint8_t changeID(uint8_t id, uint8_t newId);
@ -116,6 +117,7 @@ public:
static const byte REQUEST_VOLTAGE = 0x3E; // 1 byte 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_MOVING = 0x42; // 1 byte
static const byte REQUEST_CURRENT_CURRENT = 0x45; // 2 bytes static const byte REQUEST_CURRENT_CURRENT = 0x45; // 2 bytes