From dedaac894b605e8d832a885a4f68b17346ae36d2 Mon Sep 17 00:00:00 2001 From: Jake Date: Wed, 17 Sep 2025 01:06:58 +0800 Subject: [PATCH] added request 1/2 bytes, most data can now be requested from STS-SMS servos --- feetech.cpp | 167 +++++++++++++++++++++++++++++++++++++++---- feetech.h | 66 +++++++++++++++-- rs485transmitter.ino | 47 ++++++++++-- 3 files changed, 257 insertions(+), 23 deletions(-) diff --git a/feetech.cpp b/feetech.cpp index e8e1b06..dd9d1dc 100644 --- a/feetech.cpp +++ b/feetech.cpp @@ -94,22 +94,102 @@ void Feetech::enableTorque(uint8_t id) { setModeReceive(); } -uint16_t Feetech::getPosition(uint8_t id){ - sendRequest(id, 0x38); - waitOnReply(10); - return 0; +uint16_t Feetech::getModel(uint8_t id) { + sendRequest(id, REQUEST_MODEL, 2); + return waitOnData2Bytes(10); } -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]; - 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] = READ_DATA; // Instruction + 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] = READ_DATA; // Instruction packet[5] = instruction; // Write first address - packet[6] = 0x02; // Number of bytes to read + packet[6] = byteCount; // Number of bytes to read // Calculate checksum uint8_t sum = 0; @@ -127,7 +207,7 @@ void Feetech::pingAll() { for (int i = 0; i < 255; i++) { sendPing(i); - waitOnReply(100); + waitOnReply(10); } Serial.println("PINGING COMPLETE"); } @@ -146,6 +226,8 @@ void Feetech::waitOnReply(unsigned long timeout) { buffer[count++] = serial.read(); } + Serial.println(count); + // Display on Serial Serial.print("recv: "); 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 while (millis() - startTime < timeout) { // Loop until timeout @@ -185,6 +267,63 @@ void Feetech::waitOnReply(unsigned long timeout, uint8_t expectedReturnType) { 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 Serial.print("recv: "); Serial.print(buffer[2]); @@ -203,6 +342,8 @@ void Feetech::waitOnReply(unsigned long timeout, uint8_t expectedReturnType) { } Serial.println(); + uint16_t val = (buffer[6] * 256) + buffer[5]; + return val; break; // Exit the loop after processing the reply } diff --git a/feetech.h b/feetech.h index c61fb0a..797f3ca 100644 --- a/feetech.h +++ b/feetech.h @@ -9,12 +9,29 @@ public: Feetech(HardwareSerial& serial, int DE_PIN, int RE_PIN, int TX_PIN, int RX_PIN); void begin(); 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); - 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 pingAll(); 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); size_t receiveData(byte* buffer, size_t bufferSize); void setModeReceive(); @@ -31,10 +48,51 @@ public: static const byte SYNCWRITE_DATA = 0x83; // SIMULTANEOUS CONTROL OF MULTIPLE SERVOS 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: - HardwareSerial& serial; // Reference to the HardwareSerial object + HardwareSerial& serial; // Reference to the HardwareSerial object uint8_t DE_PIN = 20; uint8_t RE_PIN = 10; uint8_t TX_PIN = 21; diff --git a/rs485transmitter.ino b/rs485transmitter.ino index 64e1fd4..923ff45 100644 --- a/rs485transmitter.ino +++ b/rs485transmitter.ino @@ -294,22 +294,57 @@ uint16_t pos[2]; void loop() { //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) { for (int i = 0; i < 4096; i += 128) { servos.sendWritePos(103, i); servos.waitOnReply(10); - servos.getPosition(103); - Serial.println(i); + Serial.println(servos.getPosition(103)); + //Serial.println(i); delay(2000); } for (int i = 4095; i > 0; i -= 128) { servos.sendWritePos(103, i); servos.waitOnReply(10); - servos.getPosition(103); - Serial.println(i); + Serial.println(servos.getPosition(103)); + //Serial.println(i); delay(2000); } }