SCS/STS types and automatic dealing with that implemented somewhat. request ping gives id, min/max angle, and position
parent
91f6e269ed
commit
fb38ca9bdb
251
HansonServo.ino
251
HansonServo.ino
|
|
@ -19,11 +19,17 @@ uint8_t payload[MAX_PAYLOAD_SIZE]; // Global or static
|
|||
#define CMD_MESSAGE 0x06
|
||||
#define CMD_SET_POSITION 0x07
|
||||
#define CMD_PLAY_FILE 0x08
|
||||
#define CMD_SCAN_CHANNEL 0x09
|
||||
|
||||
|
||||
// ESP32 S2 PINOUT
|
||||
#define RX_PIN 18 // RO
|
||||
#define TX_PIN 17 // DI
|
||||
#define CH0_RX_PIN 13
|
||||
#define CH0_TX_PIN 12
|
||||
|
||||
// #define RX_PIN 18 // RO
|
||||
// #define TX_PIN 17 // DI
|
||||
#define CH1_RX_PIN 11 // RO
|
||||
#define CH1_TX_PIN 10 // DI
|
||||
#define DE_PIN 7 // Driver Enable
|
||||
#define RE_PIN 8 // Receiver Enable
|
||||
|
||||
|
|
@ -31,7 +37,7 @@ Animation anim;
|
|||
Animation sweep;
|
||||
Animation stagger;
|
||||
|
||||
Feetech servos = Feetech(Serial1, DE_PIN, RE_PIN, TX_PIN, RX_PIN);
|
||||
Feetech* servos[2];
|
||||
|
||||
uint16_t flipBytes(uint16_t value) {
|
||||
return (value >> 8) | (value << 8);
|
||||
|
|
@ -42,6 +48,10 @@ uint8_t ids[NUM_CHANNELS] = { 10, 11, 12, 13, 14 };
|
|||
uint16_t pos1[] = { 0, 0, 0, 0, 0 };
|
||||
uint16_t pos2[] = { 1023, 1023, 1023, 1023, 1023 };
|
||||
|
||||
uint8_t idsSTS[NUM_CHANNELS] = { 15, 103 };
|
||||
uint16_t pos1STS[] = { 0, 0 };
|
||||
uint16_t pos2STS[] = { 1023, 1023 };
|
||||
|
||||
void setup() {
|
||||
Serial.begin(1000000);
|
||||
for (int i = 0; i < 5; i++) {
|
||||
|
|
@ -49,6 +59,11 @@ void setup() {
|
|||
delay(500);
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
// pinMode(RX_PIN, OUTPUT);
|
||||
// pinMode(TX_PIN, OUTPUT);
|
||||
// pinMode(DE_PIN, OUTPUT);
|
||||
|
|
@ -63,7 +78,8 @@ void setup() {
|
|||
|
||||
pos2[3] = flipBytes(pos2[3]);
|
||||
|
||||
servos.begin();
|
||||
servos[0]->begin();
|
||||
servos[1]->begin();
|
||||
|
||||
if (!FFat.begin(true)) {
|
||||
Serial.println("FFat mount failed");
|
||||
|
|
@ -102,21 +118,35 @@ void setup() {
|
|||
|
||||
// Serial.println(anim.getFrame(0, 0)); // Should show saved value
|
||||
|
||||
//SetID(11, 14);
|
||||
//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);
|
||||
}
|
||||
|
||||
void SetID(uint8_t oldID, uint8_t newID) {
|
||||
Serial.println("Setting Lock to 0");
|
||||
Serial.println(servos.setLock(oldID, 0));
|
||||
Serial.println(servos[0]->setLockSTS(oldID, 0));
|
||||
delay(1000);
|
||||
Serial.print("Changing ID ");
|
||||
Serial.print(oldID);
|
||||
Serial.print(" to ");
|
||||
Serial.println(newID);
|
||||
Serial.println(servos.setID(oldID, newID));
|
||||
Serial.println(servos[0]->setID(oldID, newID));
|
||||
delay(1000);
|
||||
Serial.println("Setting Lock to 1");
|
||||
Serial.println(servos.setLock(newID, 1));
|
||||
Serial.println(servos[0]->setLockSTS(newID, 1));
|
||||
delay(1000);
|
||||
}
|
||||
|
||||
|
|
@ -158,6 +188,37 @@ void HandleSerialRequests() {
|
|||
}
|
||||
}
|
||||
}
|
||||
void sendMessage(const String& payload, uint8_t command = CMD_MESSAGE) {
|
||||
uint16_t length = payload.length();
|
||||
|
||||
uint8_t checksum = CMD_MESSAGE ^ (length >> 8) ^ (length & 0xFF);
|
||||
for (int i = 0; i < length; i++) {
|
||||
checksum ^= payload[i];
|
||||
}
|
||||
|
||||
Serial.write(HEADER1);
|
||||
Serial.write(HEADER2);
|
||||
Serial.write(command);
|
||||
Serial.write((length >> 8) & 0xFF);
|
||||
Serial.write(length & 0xFF);
|
||||
Serial.write((const uint8_t*)payload.c_str(), length);
|
||||
Serial.write(checksum);
|
||||
}
|
||||
|
||||
void sendMessage(const uint8_t* payload, uint16_t length, uint8_t command = CMD_MESSAGE) {
|
||||
uint8_t checksum = command ^ (length >> 8) ^ (length & 0xFF);
|
||||
for (uint16_t i = 0; i < length; i++) {
|
||||
checksum ^= payload[i];
|
||||
}
|
||||
|
||||
Serial.write(HEADER1);
|
||||
Serial.write(HEADER2);
|
||||
Serial.write(command);
|
||||
Serial.write((length >> 8) & 0xFF);
|
||||
Serial.write(length & 0xFF);
|
||||
Serial.write(payload, length);
|
||||
Serial.write(checksum);
|
||||
}
|
||||
|
||||
void handleCommand(uint8_t command, const uint8_t* payload, uint16_t length) {
|
||||
switch (command) {
|
||||
|
|
@ -187,6 +248,11 @@ void handleCommand(uint8_t command, const uint8_t* payload, uint16_t length) {
|
|||
|
||||
case CMD_PLAY_FILE:
|
||||
handlePlayAnimation(payload, length);
|
||||
break;
|
||||
|
||||
case CMD_SCAN_CHANNEL:
|
||||
handleScanChannel(payload, length);
|
||||
break;
|
||||
|
||||
default:
|
||||
Serial.print("Unknown command: ");
|
||||
|
|
@ -195,22 +261,7 @@ void handleCommand(uint8_t command, const uint8_t* payload, uint16_t length) {
|
|||
}
|
||||
}
|
||||
|
||||
void sendMessage(const String& payload, uint8_t command = CMD_MESSAGE) {
|
||||
uint16_t length = payload.length();
|
||||
|
||||
uint8_t checksum = CMD_MESSAGE ^ (length >> 8) ^ (length & 0xFF);
|
||||
for (int i = 0; i < length; i++) {
|
||||
checksum ^= payload[i];
|
||||
}
|
||||
|
||||
Serial.write(HEADER1);
|
||||
Serial.write(HEADER2);
|
||||
Serial.write(command);
|
||||
Serial.write((length >> 8) & 0xFF);
|
||||
Serial.write(length & 0xFF);
|
||||
Serial.write((const uint8_t*)payload.c_str(), length);
|
||||
Serial.write(checksum);
|
||||
}
|
||||
|
||||
void handleIdRequest() {
|
||||
String payload = String(DEVICE_NAME) + "|" + FIRMWARE_VERSION;
|
||||
|
|
@ -520,46 +571,122 @@ void handleSetPosition(const uint8_t* payload, uint16_t length) {
|
|||
|
||||
//servos.sendWritePos(ids[motorId], position);
|
||||
}
|
||||
servos.syncWritePos(ids, pos1, NUM_CHANNELS);
|
||||
// String msg = "Set positions: ";
|
||||
// for (int i = 0; i < count; i++) {
|
||||
// msg += "ID " + String(ids[i]) + " → " + String(pos1[i]) + "; ";
|
||||
// }
|
||||
// sendMessage(msg.c_str(), CMD_SET_POSITION);
|
||||
servos[0]->syncWritePos(ids, pos1, NUM_CHANNELS);
|
||||
}
|
||||
|
||||
|
||||
// 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) {
|
||||
if (length != 1) {
|
||||
sendMessage("Length of scanChannel Request Wrong");
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
for (int i = 0; i < 254; i++) {
|
||||
servos[payload[0]]->sendPing(i);
|
||||
uint8_t val = servos[payload[0]]->waitOnData1Byte(10);
|
||||
|
||||
if (val != 0) {
|
||||
uint8_t response[4];
|
||||
response[0] = payload[0]; // channel
|
||||
response[1] = i; // responding address
|
||||
uint16_t mode = servos[payload[0]]->getModel(i);
|
||||
uint16_t minAngleLimit = servos[payload[0]]->getMinAngleLimit(i);
|
||||
uint16_t maxAngleLimit = servos[payload[0]]->getMaxAngleLimit(i);
|
||||
uint16_t position = servos[payload[0]]->getPosition(i);
|
||||
response[2] = (mode >> 8) & 0xFF; // high byte
|
||||
response[3] = mode & 0xFF; // low byte
|
||||
|
||||
response[4] = (minAngleLimit >> 8) & 0xFF;
|
||||
response[5] = minAngleLimit & 0xFF;
|
||||
|
||||
response[6] = (maxAngleLimit >> 8) & 0xFF;
|
||||
response[7] = maxAngleLimit & 0xFF;
|
||||
|
||||
response[8] = (position >> 8) & 0xFF;
|
||||
response[9] = position & 0xFF;
|
||||
|
||||
|
||||
sendMessage(response, 10, CMD_SCAN_CHANNEL); // send all 4 bytes
|
||||
}
|
||||
}
|
||||
uint8_t r[2];
|
||||
r[0] = payload[0]; // channel
|
||||
r[1] = 255; // responding address
|
||||
|
||||
sendMessage(r, 2, CMD_SCAN_CHANNEL);
|
||||
|
||||
|
||||
// std::vector<uint8_t> successfulAddresses;
|
||||
// servos[payload[0]]->pingAll(successfulAddresses);
|
||||
// std::vector<uint8_t> response;
|
||||
// response.push_back(payload[0]); // channel
|
||||
// response.push_back(successfulAddresses.size()); // count
|
||||
|
||||
// for (uint8_t address : successfulAddresses) {
|
||||
// response.push_back(address);
|
||||
// }
|
||||
|
||||
// sendMessage(response.data(), response.size(), CMD_SCAN_CHANNEL);
|
||||
}
|
||||
|
||||
|
||||
|
||||
bool flip = false;
|
||||
unsigned long lastSend = 0;
|
||||
void loop() {
|
||||
|
||||
|
||||
HandleSerialRequests();
|
||||
// put your main code here, to run repeatedly:
|
||||
// servos.sendWritePos(10, 0);
|
||||
// delay(1000);
|
||||
// servos.sendWritePos(10, 200);
|
||||
// delay(1000);
|
||||
//PingAll();
|
||||
// for (int i = 0; i < 1023; i++) {
|
||||
// for (int z = 0; z < 5; z++) {
|
||||
// pos1[z] = i;
|
||||
// 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.syncWritePos(ids, pos1, 5);
|
||||
// delay(50);
|
||||
// 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);
|
||||
// }
|
||||
|
||||
// playAnimation(sweep);
|
||||
// playAnimation(stagger);
|
||||
// playLayeredAnimation(sweep, stagger);
|
||||
|
||||
|
||||
// servos.syncWritePos(ids, pos1, 5);
|
||||
// delay(1000);
|
||||
|
||||
// servos.syncWritePos(ids, pos2, 5);
|
||||
// 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 > 2000) {
|
||||
|
||||
//sendMessageFromESP32(String(millis()));
|
||||
|
|
@ -681,7 +808,7 @@ void playAnimation(Animation& anim) {
|
|||
positions[ch] = value;
|
||||
}
|
||||
Serial.println(positions[0]);
|
||||
servos.syncWritePos(ids, positions, NUM_CHANNELS);
|
||||
servos[0]->syncWritePos(ids, positions, NUM_CHANNELS);
|
||||
nextFrameTime += frameDelay;
|
||||
}
|
||||
}
|
||||
|
|
@ -704,7 +831,7 @@ void playAnimationOLD(Animation& anim) {
|
|||
|
||||
// Send frame to servos
|
||||
if (anim.getFramePositions(frame, positions)) {
|
||||
servos.syncWritePos(ids, positions, NUM_CHANNELS);
|
||||
servos[0]->syncWritePos(ids, positions, NUM_CHANNELS);
|
||||
}
|
||||
|
||||
// Schedule next frame
|
||||
|
|
@ -731,16 +858,28 @@ void playLayeredAnimation(Animation& base, Animation& overlay) {
|
|||
finalPositions[ch] = (basePositions[ch] + overlayPositions[ch]) / 2;
|
||||
}
|
||||
|
||||
servos.syncWritePos(ids, finalPositions, NUM_CHANNELS);
|
||||
servos[0]->syncWritePos(ids, finalPositions, NUM_CHANNELS);
|
||||
nextFrameTime += frameDelay;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
void PingAll() {
|
||||
void SCSPingAll() {
|
||||
std::vector<uint8_t> successfulAddresses;
|
||||
servos.pingAll(successfulAddresses);
|
||||
servos[0]->pingAll(successfulAddresses);
|
||||
|
||||
// Now successfulAddresses contains all successful pings
|
||||
Serial.println("Successful Addresses:");
|
||||
for (uint8_t address : successfulAddresses) {
|
||||
Serial.print(address);
|
||||
Serial.print(" ");
|
||||
}
|
||||
Serial.println();
|
||||
}
|
||||
|
||||
void STSPingAll() {
|
||||
std::vector<uint8_t> successfulAddresses;
|
||||
servos[1]->pingAll(successfulAddresses);
|
||||
|
||||
// Now successfulAddresses contains all successful pings
|
||||
Serial.println("Successful Addresses:");
|
||||
|
|
|
|||
281
feetech.cpp
281
feetech.cpp
|
|
@ -11,6 +11,21 @@ void Feetech::begin() {
|
|||
setModeReceive();
|
||||
}
|
||||
|
||||
void Feetech::printModel(uint16_t modelPacket) {
|
||||
switch (modelPacket) {
|
||||
case MODEL_STS3215:
|
||||
Serial.println("STS3215");
|
||||
break;
|
||||
case MODEL_STS3012:
|
||||
Serial.println("STS3012");
|
||||
break;
|
||||
default:
|
||||
Serial.print(modelPacket);
|
||||
Serial.print("\t");
|
||||
Serial.println("UNKNOWN");
|
||||
}
|
||||
}
|
||||
|
||||
void Feetech::syncWritePos(uint8_t* ids, uint16_t* positions, uint8_t count) {
|
||||
const uint8_t DATA_LEN_PER_SERVO = 0x04; // 2 bytes pos + 2 bytes speed
|
||||
|
||||
|
|
@ -52,11 +67,14 @@ void Feetech::syncWritePos(uint8_t* ids, uint16_t* positions, uint8_t count) {
|
|||
Serial1.write(checksum);
|
||||
Serial1.flush();
|
||||
setModeReceive();
|
||||
|
||||
clearEcho(sizeof(packet));
|
||||
}
|
||||
|
||||
|
||||
|
||||
// Send move command to servo id:0-255, position:0-4095
|
||||
void Feetech::sendWritePos(uint8_t id, uint16_t position) {
|
||||
uint8_t Feetech::sendWritePos(uint8_t id, uint16_t position) {
|
||||
uint8_t packet[9];
|
||||
|
||||
packet[0] = 0xFF; // Header
|
||||
|
|
@ -65,9 +83,13 @@ void Feetech::sendWritePos(uint8_t id, uint16_t position) {
|
|||
packet[3] = 5; // Length = instruction + address + 2 bytes + checksum
|
||||
packet[4] = WRITE_DATA; // Instruction: WRITE
|
||||
packet[5] = 0x2A; // Address: Goal Position
|
||||
packet[7] = (position >> 8) & 0xFF; // High byte
|
||||
packet[6] = position & 0xFF; // Low byte
|
||||
|
||||
if (feetechMode == MODE_SCS) {
|
||||
packet[6] = (position >> 8) & 0xFF; // High byte
|
||||
packet[7] = position & 0xFF; // Low byte
|
||||
} else if (feetechMode == MODE_STS) {
|
||||
packet[6] = position & 0xFF; // Low byte first for STS
|
||||
packet[7] = (position >> 8) & 0xFF; // High byte second
|
||||
}
|
||||
// Calculate checksum
|
||||
uint8_t sum = 0;
|
||||
for (int i = 2; i < 8; i++) sum += packet[i];
|
||||
|
|
@ -78,6 +100,10 @@ void Feetech::sendWritePos(uint8_t id, uint16_t position) {
|
|||
serial.write(packet, sizeof(packet));
|
||||
serial.flush();
|
||||
setModeReceive();
|
||||
clearEcho(sizeof(packet));
|
||||
|
||||
// Should later recieve a 1 byte response code packet
|
||||
return waitOnData1Byte(10);
|
||||
}
|
||||
|
||||
void Feetech::sendPing(uint8_t id) {
|
||||
|
|
@ -94,8 +120,11 @@ void Feetech::sendPing(uint8_t id) {
|
|||
for (int i = 2; i < 5; i++) sum += packet[i];
|
||||
packet[5] = ~sum; // Checksum
|
||||
|
||||
memcpy(lastSentPacket, packet, sizeof(packet));
|
||||
lastSentLength = sizeof(packet);
|
||||
// memcpy(lastSentPacket, packet, sizeof(packet));
|
||||
// lastSentLength = sizeof(packet);
|
||||
|
||||
|
||||
|
||||
|
||||
// Send packet
|
||||
//Serial.println("PING");
|
||||
|
|
@ -105,6 +134,13 @@ void Feetech::sendPing(uint8_t id) {
|
|||
serial.flush();
|
||||
//delay(20);
|
||||
setModeReceive();
|
||||
clearEcho(sizeof(packet));
|
||||
}
|
||||
|
||||
void Feetech::clearEcho(uint8_t length) {
|
||||
for (int i = 0; i < length; i++) {
|
||||
if (serial.available()) serial.read();
|
||||
}
|
||||
}
|
||||
|
||||
void Feetech::testRequest() {
|
||||
|
|
@ -120,28 +156,52 @@ void Feetech::testRequest() {
|
|||
waitOnReply(100);
|
||||
}
|
||||
|
||||
void Feetech::enableTorque(uint8_t id) {
|
||||
uint8_t packet[8]; // Adjust size based on your packet structure
|
||||
uint8_t Feetech::enableTorque(uint8_t id) {
|
||||
uint8_t packet[8];
|
||||
|
||||
packet[0] = 255; // Header
|
||||
packet[1] = 255; // Header
|
||||
packet[2] = id; // Servo ID
|
||||
packet[3] = 4; // Length (instruction + parameters + checksum)
|
||||
packet[4] = 3; // Instruction to enable torque
|
||||
packet[5] = 1; // Parameter to enable torque
|
||||
packet[0] = 0xFF; // Header
|
||||
packet[1] = 0xFF; // Header
|
||||
packet[2] = id; // Servo ID (ensure this matches the expected ID, e.g., 0x02)
|
||||
packet[3] = 0x04; // Length (4 bytes of following data)
|
||||
packet[4] = WRITE_DATA; // Instruction
|
||||
packet[5] = REQUEST_TORQUE_ENABLE; // Write first address
|
||||
packet[6] = 1; // Data to write
|
||||
|
||||
// Calculate checksum
|
||||
uint8_t sum = 0;
|
||||
for (int i = 2; i < 6; i++) {
|
||||
sum += packet[i];
|
||||
}
|
||||
packet[6] = ~sum; // Checksum (bitwise NOT)
|
||||
for (int i = 2; i < 7; i++) sum += packet[i]; // Include all data bytes for checksum
|
||||
packet[7] = ~sum; // Checksum
|
||||
|
||||
setModeTransmit();
|
||||
serial.write(packet, sizeof(packet)); // Send the packet
|
||||
serial.write(packet, 8);
|
||||
serial.flush();
|
||||
//delay(10); // Short delay
|
||||
setModeReceive();
|
||||
|
||||
return waitOnData1Byte(10);
|
||||
}
|
||||
|
||||
uint8_t Feetech::disableTorque(uint8_t id) {
|
||||
uint8_t packet[8];
|
||||
|
||||
packet[0] = 0xFF; // Header
|
||||
packet[1] = 0xFF; // Header
|
||||
packet[2] = id; // Servo ID (ensure this matches the expected ID, e.g., 0x02)
|
||||
packet[3] = 0x04; // Length (4 bytes of following data)
|
||||
packet[4] = WRITE_DATA; // Instruction
|
||||
packet[5] = REQUEST_TORQUE_ENABLE; // Write first address
|
||||
packet[6] = 0; // Data to write
|
||||
|
||||
// Calculate checksum
|
||||
uint8_t sum = 0;
|
||||
for (int i = 2; i < 7; i++) sum += packet[i]; // Include all data bytes for checksum
|
||||
packet[7] = ~sum; // Checksum
|
||||
|
||||
setModeTransmit();
|
||||
serial.write(packet, 8);
|
||||
serial.flush();
|
||||
setModeReceive();
|
||||
|
||||
return waitOnData1Byte(10);
|
||||
}
|
||||
|
||||
uint16_t Feetech::getModel(uint8_t id) {
|
||||
|
|
@ -166,13 +226,41 @@ uint8_t Feetech::getBaudRate(uint8_t id) {
|
|||
|
||||
uint16_t Feetech::getMinAngleLimit(uint8_t id) {
|
||||
sendRequest(id, REQUEST_MIN_ANGLE_LIMIT, 2);
|
||||
if (feetechMode == MODE_STS) {
|
||||
return waitOnData2Bytes(10);
|
||||
} else if (feetechMode == MODE_SCS) {
|
||||
return flipBytes(waitOnData2Bytes(10));
|
||||
}
|
||||
}
|
||||
|
||||
uint16_t Feetech::setMinAngleLimit(uint8_t id, uint16_t limit) {
|
||||
write2Bytes(id, REQUEST_MIN_ANGLE_LIMIT, limit);
|
||||
if (feetechMode == MODE_SCS) {
|
||||
return flipBytes(waitOnData2Bytes(10));
|
||||
} else if (feetechMode == MODE_STS) {
|
||||
return waitOnData2Bytes(10);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
uint16_t Feetech::getMaxAngleLimit(uint8_t id) {
|
||||
sendRequest(id, REQUEST_MAX_ANGLE_LIMIT, 2);
|
||||
if (feetechMode == MODE_SCS) {
|
||||
return flipBytes(waitOnData2Bytes(10));
|
||||
} else if (feetechMode == MODE_STS) {
|
||||
return waitOnData2Bytes(10);
|
||||
}
|
||||
}
|
||||
|
||||
uint16_t Feetech::setMaxAngleLimit(uint8_t id, uint16_t limit) {
|
||||
write2Bytes(id, REQUEST_MAX_ANGLE_LIMIT, limit);
|
||||
if (feetechMode == MODE_SCS) {
|
||||
return flipBytes(waitOnData2Bytes(10));
|
||||
} else if (feetechMode == MODE_STS) {
|
||||
return waitOnData2Bytes(10);
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t Feetech::getCWDeadZone(uint8_t id) {
|
||||
sendRequest(id, REQUEST_CW_DEAD_ZONE, 1);
|
||||
|
|
@ -242,7 +330,11 @@ float Feetech::getVoltage(uint8_t id) {
|
|||
|
||||
uint16_t Feetech::getPosition(uint8_t id) {
|
||||
sendRequest(id, REQUEST_POSITION, 2);
|
||||
if (feetechMode == MODE_STS) {
|
||||
return waitOnData2Bytes(10);
|
||||
} else if (feetechMode == MODE_SCS) {
|
||||
return flipBytes(waitOnData2Bytes(10));
|
||||
}
|
||||
}
|
||||
|
||||
int16_t Feetech::getSpeed(uint8_t id) {
|
||||
|
|
@ -297,6 +389,8 @@ void Feetech::sendRequest(uint8_t id, byte instruction, uint8_t byteCount) {
|
|||
serial.write(packet, sizeof(packet));
|
||||
serial.flush();
|
||||
setModeReceive();
|
||||
|
||||
clearEcho(sizeof(packet));
|
||||
}
|
||||
|
||||
void Feetech::write1Byte(uint8_t id, byte instruction, uint8_t data) {
|
||||
|
|
@ -316,13 +410,13 @@ void Feetech::write1Byte(uint8_t id, byte instruction, uint8_t data) {
|
|||
packet[7] = ~sum; // Checksum
|
||||
|
||||
setModeTransmit();
|
||||
serial.write(packet, sizeof(packet));
|
||||
serial.write(packet, 8);
|
||||
serial.flush();
|
||||
setModeReceive();
|
||||
}
|
||||
|
||||
void Feetech::write2Bytes(uint8_t id, byte instruction, uint16_t data) {
|
||||
uint8_t packet[8];
|
||||
uint8_t packet[9];
|
||||
|
||||
packet[0] = 0xFF; // Header
|
||||
packet[1] = 0xFF; // Header
|
||||
|
|
@ -330,16 +424,28 @@ void Feetech::write2Bytes(uint8_t id, byte instruction, uint16_t data) {
|
|||
packet[3] = 5; // Length = instruction + address + 2 bytes + checksum
|
||||
packet[4] = WRITE_DATA; // Instruction: WRITE
|
||||
packet[5] = instruction; // Address: Goal Position
|
||||
packet[7] = (data >> 8) & 0xFF; // High byte
|
||||
packet[6] = data & 0xFF; // Low byte
|
||||
if (feetechMode == MODE_SCS) {
|
||||
packet[6] = (data >> 8) & 0xFF; // High byte
|
||||
packet[7] = data & 0xFF; // Low byte
|
||||
} else if (feetechMode == MODE_STS) {
|
||||
|
||||
packet[6] = data & 0xFF; // Low byte first for STS
|
||||
packet[7] = (data >> 8) & 0xFF; // High byte second
|
||||
}
|
||||
|
||||
// Calculate checksum
|
||||
uint8_t sum = 0;
|
||||
for (int i = 2; i < 8; i++) sum += packet[i];
|
||||
for (int i = 2; i <= 7; i++) {
|
||||
sum += packet[i];
|
||||
}
|
||||
packet[8] = ~sum;
|
||||
|
||||
for (int i = 0; i < 9; i++) {
|
||||
Serial.print(packet[i], HEX);
|
||||
Serial.print(" ");
|
||||
}
|
||||
Serial.println();
|
||||
setModeTransmit();
|
||||
serial.write(packet, sizeof(packet));
|
||||
serial.write(packet, 9);
|
||||
serial.flush();
|
||||
setModeReceive();
|
||||
}
|
||||
|
|
@ -347,10 +453,10 @@ void Feetech::write2Bytes(uint8_t id, byte instruction, uint16_t data) {
|
|||
void Feetech::pingAll(std::vector<uint8_t>& successfulAddresses) {
|
||||
Serial.println("PINGING ALL 0-255");
|
||||
successfulAddresses.clear(); // Clear any previous results
|
||||
for (int i = 0; i < 20; i++) {
|
||||
for (int i = 0; i < 255; i++) {
|
||||
//Serial.println(i);
|
||||
sendPing(i);
|
||||
uint8_t val = waitOnData1Byte(50);
|
||||
uint8_t val = waitOnData1Byte(10);
|
||||
if (val != 0) {
|
||||
//Serial.println(val);
|
||||
successfulAddresses.push_back(i); // Store the successful address
|
||||
|
|
@ -412,32 +518,28 @@ uint8_t Feetech::waitOnData1Byte(unsigned long timeout) {
|
|||
buffer[count++] = serial.read();
|
||||
}
|
||||
|
||||
// Skip echoed packet if it matches the start of the buffer
|
||||
int startIndex = 0;
|
||||
if (count >= lastSentLength && memcmp(buffer, lastSentPacket, lastSentLength) == 0) {
|
||||
startIndex = lastSentLength;
|
||||
}
|
||||
|
||||
// Check if there's a valid packet after the echo
|
||||
if (count - startIndex >= 4 && buffer[startIndex] == 0xFF && buffer[startIndex + 1] == 0xFF) {
|
||||
uint8_t length = buffer[startIndex + 3];
|
||||
// Look for valid packet starting with 0xFF 0xFF
|
||||
for (int i = 0; i <= count - 4; i++) {
|
||||
if (buffer[i] == 0xFF && buffer[i + 1] == 0xFF) {
|
||||
uint8_t length = buffer[i + 3];
|
||||
int packetSize = length + 4;
|
||||
|
||||
if (startIndex + packetSize <= count) {
|
||||
Serial.print("recv: ");
|
||||
Serial.print(buffer[startIndex + 2]); // ID
|
||||
Serial.print(" ");
|
||||
Serial.print(buffer[startIndex + 4] == 0x00 ? "OK" : "NOK");
|
||||
Serial.print("\t");
|
||||
for (int i = startIndex; i < startIndex + packetSize; i++) {
|
||||
Serial.print("0x");
|
||||
if (buffer[i] < 0x10) Serial.print("0");
|
||||
Serial.print(buffer[i], HEX);
|
||||
Serial.print(" ");
|
||||
}
|
||||
Serial.println();
|
||||
if (i + packetSize <= count) {
|
||||
// Serial.print("recv: ");
|
||||
// Serial.print(buffer[i + 2]); // ID
|
||||
// Serial.print(" ");
|
||||
// Serial.print(buffer[i + 4] == 0x00 ? "OK" : "NOK");
|
||||
// Serial.print("\t");
|
||||
// for (int j = i; j < i + packetSize; j++) {
|
||||
// Serial.print("0x");
|
||||
// if (buffer[j] < 0x10) Serial.print("0");
|
||||
// Serial.print(buffer[j], HEX);
|
||||
// Serial.print(" ");
|
||||
// }
|
||||
// Serial.println();
|
||||
|
||||
return buffer[startIndex + 5]; // Return value byte
|
||||
return buffer[i + 5]; // Return value byte
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -447,56 +549,55 @@ uint8_t Feetech::waitOnData1Byte(unsigned long timeout) {
|
|||
}
|
||||
|
||||
|
||||
uint16_t Feetech::waitOnData2Bytes(unsigned long timeout) {
|
||||
unsigned long startTime = millis(); // Record the start time
|
||||
|
||||
while (millis() - startTime < timeout) { // Loop until timeout
|
||||
uint16_t Feetech::waitOnData2Bytes(unsigned long timeout) {
|
||||
unsigned long startTime = millis();
|
||||
|
||||
while (millis() - startTime < timeout) {
|
||||
if (serial.available()) {
|
||||
Serial.println("RECV");
|
||||
uint8_t buffer[32];
|
||||
int count = 0;
|
||||
|
||||
// Read all available bytes
|
||||
while (serial.available() && count < sizeof(buffer)) {
|
||||
buffer[count++] = serial.read();
|
||||
}
|
||||
|
||||
Serial.println(count);
|
||||
if (count != 8) {
|
||||
Serial.print("ERROR: Expected 8 byte reply, recieved ");
|
||||
Serial.println(count);
|
||||
return 0;
|
||||
} else {
|
||||
}
|
||||
// Look for valid packet starting with 0xFF 0xFF
|
||||
for (int i = 0; i <= count - 5; i++) {
|
||||
if (buffer[i] == 0xFF && buffer[i + 1] == 0xFF) {
|
||||
uint8_t length = buffer[i + 3];
|
||||
int packetSize = length + 4;
|
||||
|
||||
// Display on Serial
|
||||
Serial.print("recv: ");
|
||||
Serial.print(buffer[2]);
|
||||
Serial.print(" ");
|
||||
if (buffer[4] == 0x00) {
|
||||
Serial.print("OK");
|
||||
} else {
|
||||
Serial.print("NOK");
|
||||
}
|
||||
Serial.print("\t");
|
||||
for (int i = 0; i < count; i++) {
|
||||
Serial.print("0x");
|
||||
if (buffer[i] < 0x10) Serial.print("0");
|
||||
Serial.print(buffer[i], HEX);
|
||||
Serial.print(" ");
|
||||
}
|
||||
Serial.println();
|
||||
if (i + packetSize <= count) {
|
||||
// Serial.print("recv: ");
|
||||
// Serial.print(buffer[i + 2]); // ID
|
||||
// Serial.print(" ");
|
||||
// Serial.print(buffer[i + 4] == 0x00 ? "OK" : "NOK");
|
||||
// Serial.print("\t");
|
||||
// for (int j = i; j < i + packetSize; j++) {
|
||||
// Serial.print("0x");
|
||||
// if (buffer[j] < 0x10) Serial.print("0");
|
||||
// Serial.print(buffer[j], HEX);
|
||||
// Serial.print(" ");
|
||||
// }
|
||||
// Serial.println();
|
||||
|
||||
uint16_t val = (buffer[6] * 256) + buffer[5];
|
||||
return val;
|
||||
|
||||
break; // Exit the loop after processing the reply
|
||||
// Combine two bytes into a uint16_t (little-endian)
|
||||
uint16_t value = buffer[i + 5] | (buffer[i + 6] << 8);
|
||||
return value;
|
||||
}
|
||||
delay(1); // Small delay to prevent busy-waiting
|
||||
}
|
||||
}
|
||||
}
|
||||
delay(1);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void Feetech::sendData(const byte* data, size_t length) {
|
||||
// digitalWrite(_transmitPin, HIGH); // Enable transmit mode
|
||||
// _serial.write(data, length);
|
||||
|
|
@ -524,6 +625,14 @@ void Feetech::setModeReceive() {
|
|||
//delay(10);
|
||||
}
|
||||
|
||||
uint16_t Feetech::flipBytes(uint16_t value) {
|
||||
return (value >> 8) | (value << 8);
|
||||
}
|
||||
|
||||
void Feetech::setFeetechMode(FeetechMode newMode) {
|
||||
feetechMode = newMode;
|
||||
}
|
||||
|
||||
void Feetech::update() {
|
||||
if (Serial.available()) {
|
||||
setModeTransmit();
|
||||
|
|
|
|||
22
feetech.h
22
feetech.h
|
|
@ -11,15 +11,24 @@
|
|||
|
||||
class Feetech {
|
||||
public:
|
||||
enum FeetechMode {
|
||||
MODE_SCS,
|
||||
MODE_STS,
|
||||
MODE_SMS
|
||||
};
|
||||
Feetech(HardwareSerial& serial, int DE_PIN, int RE_PIN, int TX_PIN, int RX_PIN);
|
||||
void begin();
|
||||
void sendPing(uint8_t id);
|
||||
void clearEcho(uint8_t length);
|
||||
void printModel(uint16_t modelPacket);
|
||||
uint16_t getModel(uint8_t id);
|
||||
uint8_t getID(uint8_t id);
|
||||
uint8_t setID(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);
|
||||
uint16_t getMaxAngleLimit(uint8_t id);
|
||||
uint16_t setMaxAngleLimit(uint8_t id, uint16_t limit);
|
||||
uint8_t getCWDeadZone(uint8_t id);
|
||||
uint8_t getCCWDeadZone(uint8_t id);
|
||||
uint16_t getOffset(uint8_t id);
|
||||
|
|
@ -44,7 +53,7 @@ public:
|
|||
|
||||
float getVoltage(uint8_t id);
|
||||
void sendRequest(uint8_t id, uint8_t instruction, uint8_t byteCount);
|
||||
void sendWritePos(uint8_t id, uint16_t position);
|
||||
uint8_t sendWritePos(uint8_t id, uint16_t position);
|
||||
void syncWritePos(uint8_t* ids, uint16_t* positions, uint8_t count);
|
||||
void write1Byte(uint8_t id, byte instruction, uint8_t data);
|
||||
void write2Bytes(uint8_t id, byte instruction, uint16_t data);
|
||||
|
|
@ -56,9 +65,12 @@ public:
|
|||
size_t receiveData(byte* buffer, size_t bufferSize);
|
||||
void setModeReceive();
|
||||
void setModeTransmit();
|
||||
uint16_t flipBytes(uint16_t value);
|
||||
void update();
|
||||
void testRequest();
|
||||
void enableTorque(uint8_t id);
|
||||
uint8_t enableTorque(uint8_t id);
|
||||
uint8_t disableTorque(uint8_t id);
|
||||
void setFeetechMode(FeetechMode newMode);
|
||||
|
||||
static const byte PING = 0x01; // QUERY THE WORKING STATUS
|
||||
static const byte READ_DATA = 0x02; // READ DATA
|
||||
|
|
@ -110,6 +122,11 @@ public:
|
|||
static const byte SMS_STS_57600 = 6;
|
||||
static const byte SMS_STS_38400 = 7;
|
||||
|
||||
|
||||
static const uint16_t MODEL_STS3215 = 777;
|
||||
static const uint16_t MODEL_STS3012 = 521;
|
||||
static const uint16_t MODEL_SCS0009 = 1029;
|
||||
|
||||
uint8_t lastSentPacket[32];
|
||||
int lastSentLength = 0;
|
||||
|
||||
|
|
@ -120,6 +137,7 @@ private:
|
|||
uint8_t RE_PIN = 10;
|
||||
uint8_t TX_PIN = 21;
|
||||
uint8_t RX_PIN = 9;
|
||||
FeetechMode feetechMode = MODE_SCS;
|
||||
};
|
||||
|
||||
#endif // FEETECH_H
|
||||
Loading…
Reference in New Issue