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

added request 1/2 bytes, most data can now be requested from STS-SMS servos

master
Jake 2025-09-17 01:06:58 +08:00
parent 07c78bc2a0
commit dedaac894b
3 changed files with 257 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

167
feetech.cpp
View File

@ -94,22 +94,102 @@ void Feetech::enableTorque(uint8_t id) {
setModeReceive(); setModeReceive();
} }
uint16_t Feetech::getPosition(uint8_t id){ uint16_t Feetech::getModel(uint8_t id) {
sendRequest(id, 0x38); sendRequest(id, REQUEST_MODEL, 2);
waitOnReply(10); return waitOnData2Bytes(10);
return 0;
} }
void Feetech::sendRequest(uint8_t id, byte instruction) { uint8_t Feetech::getID(uint8_t id) {
sendRequest(id, REQUEST_ID, 1);
return waitOnData1Byte(10);
}
uint8_t Feetech::getBaudRate(uint8_t id) {
sendRequest(id, REQUEST_BAUD_RATE, 1);
return waitOnData1Byte(10);
}
uint16_t Feetech::getMinAngleLimit(uint8_t id) {
sendRequest(id, REQUEST_MIN_ANGLE_LIMIT, 2);
return waitOnData2Bytes(10);
}
uint16_t Feetech::getMaxAngleLimit(uint8_t id) {
sendRequest(id, REQUEST_MAX_ANGLE_LIMIT, 2);
return waitOnData2Bytes(10);
}
uint8_t Feetech::getCWDeadZone(uint8_t id) {
sendRequest(id, REQUEST_CW_DEAD_ZONE, 1);
return waitOnData1Byte(10);
}
uint8_t Feetech::getCCWDeadZone(uint8_t id) {
sendRequest(id, REQUEST_CCW_DEAD_ZONE, 1);
return waitOnData1Byte(10);
}
uint16_t Feetech::getOffset(uint8_t id) {
sendRequest(id, REQUEST_OFFSET, 2);
return waitOnData2Bytes(10);
}
uint8_t Feetech::getMode(uint8_t id) {
sendRequest(id, REQUEST_MODE, 1);
return waitOnData1Byte(10);
}
uint8_t Feetech::getTorqueEnable(uint8_t id) {
sendRequest(id, REQUEST_TORQUE_ENABLE, 1);
return waitOnData1Byte(10);
}
uint8_t Feetech::getAcceleration(uint8_t id) {
sendRequest(id, REQUEST_ACCELERATION, 1);
return waitOnData1Byte(10);
}
uint16_t Feetech::getGoalPosition(uint8_t id) {
sendRequest(id, REQUEST_GOAL_POSITION, 2);
return waitOnData2Bytes(10);
}
uint16_t Feetech::getGoalTime(uint8_t id) {
sendRequest(id, REQUEST_GOAL_TIME, 2);
return waitOnData2Bytes(10);
}
uint16_t Feetech::getGoalSpeed(uint8_t id) {
sendRequest(id, REQUEST_GOAL_TIME, 2);
return waitOnData2Bytes(10);
}
uint8_t Feetech::getLock(uint8_t id) {
sendRequest(id, REQUEST_TORQUE_ENABLE, 1);
return waitOnData1Byte(10);
}
float Feetech::getVoltage(uint8_t id) {
sendRequest(id, REQUEST_VOLTAGE, 1);
float voltage = waitOnData1Byte(10) * 0.1;
return voltage;
}
uint16_t Feetech::getPosition(uint8_t id) {
sendRequest(id, REQUEST_POSITION, 2);
return waitOnData2Bytes(10);
}
void Feetech::sendRequest(uint8_t id, byte instruction, uint8_t byteCount) {
uint8_t packet[8]; uint8_t packet[8];
packet[0] = 0xFF; // Header packet[0] = 0xFF; // Header
packet[1] = 0xFF; // Header packet[1] = 0xFF; // Header
packet[2] = id; // Servo ID (ensure this matches the expected ID, e.g., 0x02) packet[2] = id; // Servo ID (ensure this matches the expected ID, e.g., 0x02)
packet[3] = 0x04; // Length (4 bytes of following data) packet[3] = 0x04; // Length (4 bytes of following data)
packet[4] = READ_DATA; // Instruction packet[4] = READ_DATA; // Instruction
packet[5] = instruction; // Write first address packet[5] = instruction; // Write first address
packet[6] = 0x02; // Number of bytes to read packet[6] = byteCount; // Number of bytes to read
// Calculate checksum // Calculate checksum
uint8_t sum = 0; uint8_t sum = 0;
@ -127,7 +207,7 @@ void Feetech::pingAll() {
for (int i = 0; i < 255; i++) { for (int i = 0; i < 255; i++) {
sendPing(i); sendPing(i);
waitOnReply(100); waitOnReply(10);
} }
Serial.println("PINGING COMPLETE"); Serial.println("PINGING COMPLETE");
} }
@ -146,6 +226,8 @@ void Feetech::waitOnReply(unsigned long timeout) {
buffer[count++] = serial.read(); buffer[count++] = serial.read();
} }
Serial.println(count);
// Display on Serial // Display on Serial
Serial.print("recv: "); Serial.print("recv: ");
Serial.print(buffer[2]); Serial.print(buffer[2]);
@ -171,7 +253,7 @@ void Feetech::waitOnReply(unsigned long timeout) {
} }
} }
void Feetech::waitOnReply(unsigned long timeout, uint8_t expectedReturnType) { uint8_t Feetech::waitOnData1Byte(unsigned long timeout) {
unsigned long startTime = millis(); // Record the start time unsigned long startTime = millis(); // Record the start time
while (millis() - startTime < timeout) { // Loop until timeout while (millis() - startTime < timeout) { // Loop until timeout
@ -185,6 +267,63 @@ void Feetech::waitOnReply(unsigned long timeout, uint8_t expectedReturnType) {
buffer[count++] = serial.read(); buffer[count++] = serial.read();
} }
Serial.println(count);
// if (count != 8) {
// Serial.print("ERROR: Expected 8 byte reply, recieved ");
// Serial.println(count);
// return 0;
// } else {
// }
// 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();
uint8_t val = buffer[5];
return val;
break; // Exit the loop after processing the reply
}
delay(10); // Small delay to prevent busy-waiting
}
}
uint16_t Feetech::waitOnData2Bytes(unsigned long timeout) {
unsigned long startTime = millis(); // Record the start time
while (millis() - startTime < timeout) { // Loop until 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 {
}
// Display on Serial // Display on Serial
Serial.print("recv: "); Serial.print("recv: ");
Serial.print(buffer[2]); Serial.print(buffer[2]);
@ -203,6 +342,8 @@ void Feetech::waitOnReply(unsigned long timeout, uint8_t expectedReturnType) {
} }
Serial.println(); Serial.println();
uint16_t val = (buffer[6] * 256) + buffer[5];
return val;
break; // Exit the loop after processing the reply break; // Exit the loop after processing the reply
} }

66
feetech.h
View File

@ -9,12 +9,29 @@ public:
Feetech(HardwareSerial& serial, int DE_PIN, int RE_PIN, int TX_PIN, int RX_PIN); Feetech(HardwareSerial& serial, int DE_PIN, int RE_PIN, int TX_PIN, int RX_PIN);
void begin(); void begin();
void sendPing(uint8_t id); void sendPing(uint8_t id);
uint16_t getModel(uint8_t id);
uint8_t getID(uint8_t id);
uint8_t getBaudRate(uint8_t id);
uint16_t getMinAngleLimit(uint8_t id);
uint16_t getMaxAngleLimit(uint8_t id);
uint8_t getCWDeadZone(uint8_t id);
uint8_t getCCWDeadZone(uint8_t id);
uint16_t getOffset(uint8_t id);
uint8_t getMode(uint8_t id);
uint8_t getTorqueEnable(uint8_t id);
uint8_t getAcceleration(uint8_t id);
uint16_t getGoalPosition(uint8_t id);
uint16_t getGoalTime(uint8_t id);
uint16_t getGoalSpeed(uint8_t id);
uint8_t getLock(uint8_t id);
uint16_t getPosition(uint8_t id); uint16_t getPosition(uint8_t id);
void sendRequest(uint8_t id, uint8_t instruction); float getVoltage(uint8_t id);
void sendRequest(uint8_t id, uint8_t instruction, uint8_t byteCount);
void sendWritePos(uint8_t id, uint16_t position); void sendWritePos(uint8_t id, uint16_t position);
void pingAll(); void pingAll();
void waitOnReply(unsigned long timeout); void waitOnReply(unsigned long timeout);
void waitOnReply(unsigned long timeout, uint8_t expectedReturnType); uint8_t waitOnData1Byte(unsigned long timeout);
uint16_t waitOnData2Bytes(unsigned long timeout);
void sendData(const byte* data, size_t length); void sendData(const byte* data, size_t length);
size_t receiveData(byte* buffer, size_t bufferSize); size_t receiveData(byte* buffer, size_t bufferSize);
void setModeReceive(); void setModeReceive();
@ -31,10 +48,51 @@ public:
static const byte SYNCWRITE_DATA = 0x83; // SIMULTANEOUS CONTROL OF MULTIPLE SERVOS static const byte SYNCWRITE_DATA = 0x83; // SIMULTANEOUS CONTROL OF MULTIPLE SERVOS
static const byte RESET = 0x06; // RESET TO FACTORY DEFAULT static const byte RESET = 0x06; // RESET TO FACTORY DEFAULT
static const byte REQUEST_POSITION = 0x38; // Current Position request
// MEMORY TABLE LOCATIONS SMS-STS
static const byte REQUEST_MODEL = 0x03; // 2 bytes
static const byte REQUEST_ID = 0x05; // 1 byte
static const byte REQUEST_BAUD_RATE = 0x06; // 1 byte
static const byte REQUEST_MIN_ANGLE_LIMIT = 0x09; // 2 bytes
static const byte REQUEST_MAX_ANGLE_LIMIT = 0x0B; // 2 bytes
static const byte REQUEST_CW_DEAD_ZONE = 0x1A; // 1 byte
static const byte REQUEST_CCW_DEAD_ZONE = 0x1B; // 1 byte
static const byte REQUEST_OFFSET = 0x1F; // 2 bytes
static const byte REQUEST_MODE = 0x21; // 1 byte
static const byte REQUEST_TORQUE_ENABLE = 0x28; // 1 byte
static const byte REQUEST_ACCELERATION = 0x29; // 1 byte
static const byte REQUEST_GOAL_POSITION = 0x2A; // 2 byte
static const byte REQUEST_GOAL_TIME = 0x2C; // 2 byte
static const byte REQUEST_GOAL_SPEED = 0x2E; // 2 byte
static const byte REQUEST_LOCK = 0x37; // 1 byte
static const byte REQUEST_POSITION = 0x38; // 2 bytes
static const byte REQUEST_VOLTAGE = 0x3E; // 1 byte
// TODO
// SMS_STS_PRESENT_SPEED_L = 0x3A
// SMS_STS_PRESENT_SPEED_H = 0x3B
// SMS_STS_PRESENT_LOAD_L = 0x3C
// SMS_STS_PRESENT_LOAD_H = 0x3D
// SMS_STS_PRESENT_VOLTAGE = 0x3E
// SMS_STS_PRESENT_TEMPERATURE = 0x3F
// SMS_STS_MOVING = 0x42
// SMS_STS_PRESENT_CURRENT_L = 0x45
// SMS_STS_PRESENT_CURRENT_H = 0x46
// BAUD RATES (stored as 1 byte)
static const byte SMS_STS_1M = 0;
static const byte SMS_STS_0_5M = 1;
static const byte SMS_STS_250K = 2;
static const byte SMS_STS_128K = 3;
static const byte SMS_STS_115200 = 4;
static const byte SMS_STS_76800 = 5;
static const byte SMS_STS_57600 = 6;
static const byte SMS_STS_38400 = 7;
private: private:
HardwareSerial& serial; // Reference to the HardwareSerial object HardwareSerial& serial; // Reference to the HardwareSerial object
uint8_t DE_PIN = 20; uint8_t DE_PIN = 20;
uint8_t RE_PIN = 10; uint8_t RE_PIN = 10;
uint8_t TX_PIN = 21; uint8_t TX_PIN = 21;

45
rs485transmitter.ino
View File

@ -294,22 +294,57 @@ uint16_t pos[2];
void loop() { void loop() {
//PingAll(); //PingAll();
//servos.sendWritePos(103, 4095);
for (int i = 0; i < 50; i++) {
Serial.println(servos.getAcceleration(103));
delay(5);
}
//servos.sendWritePos(103, 0);
for (int i = 0; i < 50; i++) {
Serial.println(servos.getAcceleration(103));
delay(5);
}
return;
Serial.println("getTorqueEnable");
Serial.println(servos.getTorqueEnable(103));
delay(1000);
Serial.println("getAcceleration");
Serial.println(servos.getAcceleration(103));
delay(1000);
Serial.println("getGoalPosition");
Serial.println(servos.getGoalPosition(103));
delay(1000);
Serial.println("getGoalTime");
Serial.println(servos.getGoalTime(103));
delay(1000);
Serial.println("getGoalSpeed");
Serial.println(servos.getGoalSpeed(103));
delay(1000);
Serial.println("getLock");
Serial.println(servos.getLock(103));
delay(1000);
return;
// uint8_t getTorqueEnable(uint8_t id);
// uint8_t getAcceleration(uint8_t id);
// uint16_t getGoalPosition(uint8_t id);
// uint16_t getGoalTime(uint8_t id);
// uint16_t getGoalSpeed(uint8_t id);
// uint8_t getLock(uint8_t id);
if (true) { if (true) {
for (int i = 0; i < 4096; i += 128) { for (int i = 0; i < 4096; i += 128) {
servos.sendWritePos(103, i); servos.sendWritePos(103, i);
servos.waitOnReply(10); servos.waitOnReply(10);
servos.getPosition(103); Serial.println(servos.getPosition(103));
Serial.println(i); //Serial.println(i);
delay(2000); delay(2000);
} }
for (int i = 4095; i > 0; i -= 128) { for (int i = 4095; i > 0; i -= 128) {
servos.sendWritePos(103, i); servos.sendWritePos(103, i);
servos.waitOnReply(10); servos.waitOnReply(10);
servos.getPosition(103); Serial.println(servos.getPosition(103));
Serial.println(i); //Serial.println(i);
delay(2000); delay(2000);
} }
} }