first
commit
03e1591388
|
|
@ -0,0 +1,65 @@
|
|||
#include "feetech.h"
|
||||
// ESP32 S2 PINOUT
|
||||
#define RX_PIN 17 // DI
|
||||
#define TX_PIN 18 // RO
|
||||
#define DE_PIN 33 // Driver Enable
|
||||
#define RE_PIN 3 // Receiver Enable
|
||||
|
||||
|
||||
Feetech servos = Feetech(Serial1, DE_PIN, RE_PIN, TX_PIN, RX_PIN);
|
||||
|
||||
uint16_t flipBytes(uint16_t value) {
|
||||
return (value >> 8) | (value << 8);
|
||||
}
|
||||
|
||||
uint8_t ids[] = { 1, 10, 11, 15 };
|
||||
uint16_t pos1[] = { 0, 0, 0, 0 };
|
||||
uint16_t pos2[] = { 1023, 1023, 1023, 4095 };
|
||||
|
||||
|
||||
void setup() {
|
||||
Serial.begin(115200);
|
||||
for (int i = 0; i < 10; i++) {
|
||||
Serial.println(i);
|
||||
delay(500);
|
||||
}
|
||||
|
||||
pos2[3] = flipBytes(pos2[3]);
|
||||
|
||||
servos.begin();
|
||||
|
||||
}
|
||||
|
||||
void SetID(uint8_t oldID, uint8_t newID) {
|
||||
Serial.println("Setting Lock to 0");
|
||||
Serial.println(servos.setLock(oldID, 0));
|
||||
delay(1000);
|
||||
Serial.print("Changing ID ");
|
||||
Serial.print(oldID);
|
||||
Serial.print(" to ");
|
||||
Serial.println(newID);
|
||||
Serial.println(servos.setID(oldID, newID));
|
||||
delay(1000);
|
||||
Serial.println("Setting Lock to 1");
|
||||
Serial.println(servos.setLock(newID, 1));
|
||||
delay(1000);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// put your main code here, to run repeatedly:
|
||||
PingAll();
|
||||
|
||||
}
|
||||
|
||||
void PingAll() {
|
||||
std::vector<uint8_t> successfulAddresses;
|
||||
servos.pingAll(successfulAddresses);
|
||||
|
||||
// Now successfulAddresses contains all successful pings
|
||||
Serial.println("Successful Addresses:");
|
||||
for (uint8_t address : successfulAddresses) {
|
||||
Serial.print(address);
|
||||
Serial.print(" ");
|
||||
}
|
||||
Serial.println();
|
||||
}
|
||||
|
|
@ -0,0 +1,546 @@
|
|||
#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();
|
||||
}
|
||||
|
||||
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
|
||||
|
||||
// Calculate packet length: instruction + address + data length + (count × data per servo)
|
||||
uint8_t packetLen = 4 + (count * (1 + DATA_LEN_PER_SERVO)); // instruction + addr + len + servo data
|
||||
|
||||
// Create packet buffer
|
||||
uint8_t packet[3 + packetLen]; // 3 header bytes + payload
|
||||
uint8_t index = 0;
|
||||
|
||||
// Header
|
||||
packet[index++] = 0xFF;
|
||||
packet[index++] = 0xFF;
|
||||
packet[index++] = BROADCAST_ID;
|
||||
packet[index++] = packetLen;
|
||||
packet[index++] = SYNCWRITE_DATA;
|
||||
packet[index++] = REQUEST_GOAL_POSITION;
|
||||
packet[index++] = DATA_LEN_PER_SERVO;
|
||||
|
||||
// Servo data
|
||||
for (uint8_t i = 0; i < count; i++) {
|
||||
packet[index++] = ids[i];
|
||||
packet[index++] = (uint8_t)((positions[i] >> 8) & 0xFF); // High byte
|
||||
packet[index++] = (uint8_t)(positions[i] & 0xFF); // Low byte
|
||||
packet[index++] = 0x00; // Speed high byte
|
||||
packet[index++] = 0x00; // Speed low byte
|
||||
}
|
||||
|
||||
// Checksum
|
||||
uint8_t checksum = 0;
|
||||
for (uint8_t i = 2; i < index; i++) {
|
||||
checksum += packet[i];
|
||||
}
|
||||
checksum = ~checksum;
|
||||
|
||||
// Send packet
|
||||
setModeTransmit();
|
||||
Serial1.write(packet, index);
|
||||
Serial1.write(checksum);
|
||||
Serial1.flush();
|
||||
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
|
||||
//Serial.println("PING");
|
||||
setModeTransmit();
|
||||
//delay(20);
|
||||
serial.write(packet, sizeof(packet));
|
||||
serial.flush();
|
||||
//delay(20);
|
||||
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::setID(uint8_t id, uint8_t newId) {
|
||||
write1Byte(id, REQUEST_ID, newId);
|
||||
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_LOCK, 1);
|
||||
return waitOnData1Byte(10);
|
||||
}
|
||||
|
||||
uint8_t Feetech::setLock(uint8_t id, uint8_t lockEnabled) {
|
||||
write1Byte(id, REQUEST_LOCK, lockEnabled);
|
||||
return waitOnData1Byte(10);
|
||||
}
|
||||
|
||||
uint8_t Feetech::setLockSTS(uint8_t id, uint8_t lockEnabled) {
|
||||
write1Byte(id, 0x37, lockEnabled);
|
||||
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::write1Byte(uint8_t id, byte instruction, uint8_t data) {
|
||||
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] = instruction; // Write first address
|
||||
packet[6] = data; // 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, sizeof(packet));
|
||||
serial.flush();
|
||||
setModeReceive();
|
||||
}
|
||||
|
||||
void Feetech::write2Bytes(uint8_t id, byte instruction, uint16_t data) {
|
||||
uint8_t packet[8];
|
||||
|
||||
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] = instruction; // Address: Goal Position
|
||||
packet[7] = (data >> 8) & 0xFF; // High byte
|
||||
packet[6] = data & 0xFF; // Low byte
|
||||
|
||||
// Calculate checksum
|
||||
uint8_t sum = 0;
|
||||
for (int i = 2; i < 8; i++) sum += packet[i];
|
||||
packet[8] = ~sum;
|
||||
|
||||
setModeTransmit();
|
||||
serial.write(packet, sizeof(packet));
|
||||
serial.flush();
|
||||
setModeReceive();
|
||||
}
|
||||
|
||||
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 < 254; i++) {
|
||||
//Serial.println(i);
|
||||
sendPing(i);
|
||||
uint8_t val = waitOnData1Byte(50);
|
||||
if (val != 0) {
|
||||
//Serial.println(val);
|
||||
successfulAddresses.push_back(i); // Store the successful address
|
||||
}
|
||||
}
|
||||
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
|
||||
} else {
|
||||
//Serial.println(millis() - startTime);
|
||||
}
|
||||
delay(1); // Small delay to prevent busy-waiting
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
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(1); // Small delay to prevent busy-waiting
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
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
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,121 @@
|
|||
#ifndef FEETECH_H
|
||||
#define FEETECH_H
|
||||
|
||||
#include <vector>
|
||||
#include <Arduino.h>
|
||||
#include <HardwareSerial.h>
|
||||
|
||||
|
||||
// SCS & HLS big-endian (high byte first)
|
||||
// STS little-endian (low byte first)
|
||||
|
||||
class Feetech {
|
||||
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 setID(uint8_t id, uint8_t newId);
|
||||
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);
|
||||
uint8_t setLock(uint8_t id, uint8_t lockEnabled);
|
||||
uint8_t setLockSTS(uint8_t id, uint8_t lockEnabled);
|
||||
uint16_t getPosition(uint8_t id);
|
||||
|
||||
int16_t getSpeed(uint8_t id);
|
||||
uint16_t getLoad(uint8_t id);
|
||||
uint8_t getTemperature(uint8_t id);
|
||||
|
||||
|
||||
uint8_t getMoving(uint8_t id);
|
||||
float getCurrent(uint8_t id);
|
||||
|
||||
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 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);
|
||||
void pingAll(std::vector<uint8_t>& successfulAddresses);
|
||||
void waitOnReply(unsigned long timeout);
|
||||
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();
|
||||
void setModeTransmit();
|
||||
void update();
|
||||
void testRequest();
|
||||
void enableTorque(uint8_t id);
|
||||
|
||||
static const byte PING = 0x01; // QUERY THE WORKING STATUS
|
||||
static const byte READ_DATA = 0x02; // READ DATA
|
||||
static const byte WRITE_DATA = 0x03; // WRITE DATA
|
||||
static const byte REGWRITE_DATA = 0x04; // QUEUES MOVES FOR ACTION COMMAND
|
||||
static const byte ACTION = 0x05; // TRIGGERS REG WRITE WRITES
|
||||
static const byte SYNCWRITE_DATA = 0x83; // SIMULTANEOUS CONTROL OF MULTIPLE SERVOS
|
||||
static const byte RESET = 0x06; // RESET TO FACTORY DEFAULT
|
||||
static const byte BROADCAST_ID = 0xFE;
|
||||
|
||||
|
||||
// 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 = 0x30; // 1 byte
|
||||
|
||||
static const byte REQUEST_POSITION = 0x38; // 2 bytes
|
||||
|
||||
static const byte REQUEST_CURRENT_SPEED = 0x3A; // 2 bytes
|
||||
static const byte REQUEST_CURRENT_LOAD = 0x3C; // 2 bytes
|
||||
|
||||
static const byte REQUEST_VOLTAGE = 0x3E; // 1 byte
|
||||
|
||||
static const byte REQUEST_TEMPERATURE = 0x3F; // 1 byte
|
||||
static const byte REQUEST_MOVING = 0x42; // 1 byte
|
||||
static const byte REQUEST_CURRENT_CURRENT = 0x45; // 2 bytes
|
||||
|
||||
|
||||
// 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
|
||||
uint8_t DE_PIN = 20;
|
||||
uint8_t RE_PIN = 10;
|
||||
uint8_t TX_PIN = 21;
|
||||
uint8_t RX_PIN = 9;
|
||||
};
|
||||
|
||||
#endif // FEETECH_H
|
||||
Loading…
Reference in New Issue