added request 1/2 bytes, most data can now be requested from STS-SMS servos
parent
07c78bc2a0
commit
dedaac894b
167
feetech.cpp
167
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
|
||||
}
|
||||
|
|
|
|||
66
feetech.h
66
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;
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue