430 lines
11 KiB
C++
430 lines
11 KiB
C++
#include "feetech.h"
|
|
|
|
Feetech::Feetech(HardwareSerial& serial, int DE_PIN, int RE_PIN, int TX_PIN, int RX_PIN)
|
|
: serial(serial), DE_PIN(DE_PIN), RE_PIN(RE_PIN), TX_PIN(TX_PIN), RX_PIN(RX_PIN) {
|
|
}
|
|
|
|
void Feetech::begin() {
|
|
serial.begin(1000000, SERIAL_8N1, RX_PIN, TX_PIN);
|
|
pinMode(DE_PIN, OUTPUT);
|
|
pinMode(RE_PIN, OUTPUT);
|
|
setModeReceive();
|
|
}
|
|
|
|
// Send move command to servo id:0-255, position:0-4095
|
|
void Feetech::sendWritePos(uint8_t id, uint16_t position) {
|
|
uint8_t packet[9];
|
|
|
|
packet[0] = 0xFF; // Header
|
|
packet[1] = 0xFF; // Header
|
|
packet[2] = id; // Servo ID
|
|
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
|
|
|
|
// Calculate checksum
|
|
uint8_t sum = 0;
|
|
for (int i = 2; i < 8; i++) sum += packet[i];
|
|
packet[8] = ~sum;
|
|
|
|
// Send packet
|
|
setModeTransmit();
|
|
serial.write(packet, sizeof(packet));
|
|
serial.flush();
|
|
setModeReceive();
|
|
}
|
|
|
|
void Feetech::sendPing(uint8_t id) {
|
|
uint8_t packet[6];
|
|
|
|
packet[0] = 0xFF; // Header
|
|
packet[1] = 0xFF; // Header
|
|
packet[2] = id; // Servo ID
|
|
packet[3] = 0x02; // Length = instruction + address + checksum
|
|
packet[4] = PING; // Instruction: PING
|
|
|
|
// Calculate checksum
|
|
uint8_t sum = 0;
|
|
for (int i = 2; i < 5; i++) sum += packet[i];
|
|
packet[5] = ~sum; // Checksum
|
|
|
|
// Send packet
|
|
setModeTransmit();
|
|
serial.write(packet, sizeof(packet));
|
|
serial.flush();
|
|
setModeReceive();
|
|
}
|
|
|
|
void Feetech::testRequest() {
|
|
uint8_t packet[6] = { 0XFF, 0XFF, 0X00, 0X02, 0X06, 0XF7 };
|
|
uint8_t sum = 0;
|
|
for (int i = 2; i < 5; i++) sum += packet[i]; // Include all data bytes for checksum
|
|
packet[5] = ~sum; // Checksum
|
|
|
|
setModeTransmit();
|
|
serial.write(packet, sizeof(packet));
|
|
serial.flush();
|
|
setModeReceive();
|
|
waitOnReply(100);
|
|
}
|
|
|
|
void Feetech::enableTorque(uint8_t id) {
|
|
uint8_t packet[8]; // Adjust size based on your packet structure
|
|
|
|
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
|
|
|
|
// Calculate checksum
|
|
uint8_t sum = 0;
|
|
for (int i = 2; i < 6; i++) {
|
|
sum += packet[i];
|
|
}
|
|
packet[6] = ~sum; // Checksum (bitwise NOT)
|
|
|
|
setModeTransmit();
|
|
serial.write(packet, sizeof(packet)); // Send the packet
|
|
serial.flush();
|
|
//delay(10); // Short delay
|
|
setModeReceive();
|
|
}
|
|
|
|
uint16_t Feetech::getModel(uint8_t id) {
|
|
sendRequest(id, REQUEST_MODEL, 2);
|
|
return waitOnData2Bytes(10);
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
int16_t Feetech::getSpeed(uint8_t id) {
|
|
sendRequest(id, REQUEST_CURRENT_SPEED, 2);
|
|
int16_t val = waitOnData2Bytes(10);
|
|
if (val < 0){
|
|
val -= 32767;
|
|
val = -val;
|
|
}
|
|
return val;
|
|
}
|
|
|
|
// NOT SURE ABOUT THIS ONE from 0-1000 one direction, 0-2024 the other.
|
|
uint16_t Feetech::getLoad(uint8_t id) {
|
|
sendRequest(id, REQUEST_CURRENT_LOAD, 2);
|
|
return waitOnData2Bytes(10);
|
|
}
|
|
|
|
uint8_t Feetech::getTemperature(uint8_t id) {
|
|
sendRequest(id, REQUEST_TEMPERATURE, 1);
|
|
return waitOnData1Byte(10);
|
|
}
|
|
|
|
uint8_t Feetech::getMoving(uint8_t id) {
|
|
sendRequest(id, REQUEST_MOVING, 1);
|
|
return waitOnData1Byte(10);
|
|
}
|
|
|
|
// Multiplier could be wrong
|
|
float Feetech::getCurrent(uint8_t id) {
|
|
sendRequest(id, REQUEST_CURRENT_CURRENT, 2);
|
|
return waitOnData2Bytes(10) * 0.01;
|
|
}
|
|
|
|
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[5] = instruction; // Write first address
|
|
packet[6] = byteCount; // Number of bytes to read
|
|
|
|
// 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, sizeof(packet));
|
|
serial.flush();
|
|
setModeReceive();
|
|
}
|
|
|
|
void Feetech::pingAll() {
|
|
Serial.println("PINGING ALL 0-255");
|
|
for (int i = 0; i < 255; i++) {
|
|
sendPing(i);
|
|
|
|
waitOnReply(50);
|
|
}
|
|
Serial.println("PINGING COMPLETE");
|
|
}
|
|
|
|
void Feetech::waitOnReply(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);
|
|
|
|
// 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();
|
|
|
|
|
|
break; // Exit the loop after processing the reply
|
|
}
|
|
delay(10); // Small delay to prevent busy-waiting
|
|
}
|
|
}
|
|
|
|
uint8_t Feetech::waitOnData1Byte(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]);
|
|
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]);
|
|
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();
|
|
|
|
uint16_t val = (buffer[6] * 256) + buffer[5];
|
|
return val;
|
|
|
|
break; // Exit the loop after processing the reply
|
|
}
|
|
delay(10); // Small delay to prevent busy-waiting
|
|
}
|
|
}
|
|
|
|
void Feetech::sendData(const byte* data, size_t length) {
|
|
// digitalWrite(_transmitPin, HIGH); // Enable transmit mode
|
|
// _serial.write(data, length);
|
|
// _serial.flush(); // Wait for transmission to complete
|
|
// digitalWrite(_transmitPin, LOW); // Disable transmit mode
|
|
}
|
|
|
|
size_t Feetech::receiveData(byte* buffer, size_t bufferSize) {
|
|
size_t bytesRead = 0;
|
|
while (serial.available() > 0 && bytesRead < bufferSize) {
|
|
buffer[bytesRead++] = serial.read();
|
|
}
|
|
return bytesRead;
|
|
}
|
|
|
|
void Feetech::setModeTransmit() {
|
|
digitalWrite(DE_PIN, HIGH);
|
|
digitalWrite(RE_PIN, HIGH);
|
|
delay(10);
|
|
}
|
|
|
|
void Feetech::setModeReceive() {
|
|
digitalWrite(DE_PIN, LOW);
|
|
digitalWrite(RE_PIN, LOW);
|
|
delay(10);
|
|
}
|
|
|
|
void Feetech::update() {
|
|
if (Serial.available()) {
|
|
setModeTransmit();
|
|
while (Serial.available()) {
|
|
char incomingByte = Serial.read(); // Read from USB Serial
|
|
serial.write(incomingByte); // Send to Serial1
|
|
}
|
|
setModeReceive();
|
|
}
|
|
|
|
// Pass data from Serial1 (Feetech) to Serial (USB)
|
|
if (serial.available()) {
|
|
while (serial.available()) {
|
|
char incomingByte = serial.read(); // Read from Serial1
|
|
Serial.write(incomingByte); // Send to USB Serial
|
|
}
|
|
}
|
|
} |