470 lines
12 KiB
C++
470 lines
12 KiB
C++
#include "feetech.h"
|
||
|
||
|
||
|
||
bool PRINT_SENT_PACKETS = false;
|
||
// S3 setup
|
||
// #define RX_PIN 4 // DI
|
||
// #define DE_PIN 5 // Driver Enable
|
||
// #define RE_PIN 6 // Receiver Enable
|
||
// #define TX_PIN 7 // RO
|
||
|
||
|
||
// C3 setup
|
||
#define DE_PIN 20
|
||
#define RE_PIN 10
|
||
#define TX_PIN 21
|
||
#define RX_PIN 9
|
||
|
||
Feetech servos = Feetech(Serial1, DE_PIN, RE_PIN, TX_PIN, RX_PIN);
|
||
|
||
// SCS are BIG endian (high byte first)
|
||
// SMS are little endian (low byte first)
|
||
|
||
|
||
#define PING 0x01 // QUERY THE WORKING STATUS
|
||
#define READ_DATA 0x02 // READ DATA
|
||
#define WRITE_DATA 0x03 // WRITE DATA
|
||
#define REGWRITE_DATA 0x04 // QUEUES MOVES FOR ACTION COMMAND
|
||
#define ACTION 0x05 // TRIGGERS REG WRITE WRITES
|
||
#define SYNCWRITE_DATA 0x83 // SIMULTANEOUS CONTROL OF MULTIPLE SERVOS
|
||
#define RESET 0x06 // RESET TO FACTORY DEFAULT
|
||
|
||
void setup() {
|
||
Serial.begin(115200);
|
||
servos.begin();
|
||
|
||
|
||
// // while (true){
|
||
// // digitalWrite(TX_PIN, !digitalRead(TX_PIN));
|
||
// // Serial.println(digitalRead(TX_PIN));
|
||
// // delay(2000);
|
||
// // }
|
||
|
||
// Serial1.begin(1000000, SERIAL_8N1, RX_PIN, TX_PIN);
|
||
// pinMode(DE_PIN, OUTPUT);
|
||
// pinMode(RE_PIN, OUTPUT);
|
||
// SetModeReceive();
|
||
// delay(1000);
|
||
}
|
||
|
||
// Send move command to servo id:0-255, position:0-4095
|
||
void 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();
|
||
Serial1.write(packet, sizeof(packet));
|
||
delay(10);
|
||
//Serial1.flush();
|
||
SetModeReceive();
|
||
}
|
||
|
||
|
||
void syncWriteTwoServos(uint16_t pos1, uint16_t pos2) {
|
||
Serial.println(pos1);
|
||
const uint8_t START_BYTE = 0xFF;
|
||
const uint8_t BROADCAST_ID = 0xFE;
|
||
const uint8_t SYNC_WRITE = 0x83;
|
||
const uint8_t START_ADDR = 0x2A;
|
||
const uint8_t DATA_LEN_PER_SERVO = 6; // pos(2) + time(2) + speed(2)
|
||
const uint8_t NUM_SERVOS = 2;
|
||
|
||
uint8_t packetLen = 4 + (NUM_SERVOS * (1 + DATA_LEN_PER_SERVO)); // instruction + addr + len + servo data
|
||
uint8_t packet[3 + packetLen]; // header + payload
|
||
uint8_t index = 0;
|
||
|
||
// Header
|
||
packet[index++] = START_BYTE;
|
||
packet[index++] = START_BYTE;
|
||
packet[index++] = BROADCAST_ID;
|
||
packet[index++] = packetLen;
|
||
packet[index++] = SYNC_WRITE;
|
||
packet[index++] = START_ADDR;
|
||
packet[index++] = DATA_LEN_PER_SERVO;
|
||
|
||
// Servo 1
|
||
packet[index++] = 0x01; // ID
|
||
packet[index++] = (pos1 >> 8) & 0xFF; // Position high
|
||
packet[index++] = pos1 & 0xFF; // Position low
|
||
packet[index++] = 0x00; // Time high
|
||
packet[index++] = 0x00; // Time low
|
||
packet[index++] = 0x07; // High byte
|
||
packet[index++] = 0xD0; // Low byte 2000
|
||
|
||
|
||
// Servo 2
|
||
packet[index++] = 0x02; // ID
|
||
packet[index++] = pos2 & 0xFF;
|
||
packet[index++] = (pos2 >> 8) & 0xFF;
|
||
packet[index++] = 0x00;
|
||
packet[index++] = 0x00;
|
||
packet[index++] = 0xE8;
|
||
packet[index++] = 0x03;
|
||
|
||
// Checksum
|
||
uint8_t checksum = 0;
|
||
for (uint8_t i = 2; i < index; i++) {
|
||
checksum += packet[i];
|
||
}
|
||
checksum = ~checksum & 0xFF;
|
||
|
||
// Send packet
|
||
SetModeTransmit();
|
||
Serial1.write(packet, index);
|
||
Serial1.write(checksum);
|
||
Serial1.flush();
|
||
delay(2);
|
||
SetModeReceive();
|
||
}
|
||
|
||
|
||
|
||
void syncWriteServos(uint8_t* ids, uint16_t* positions, uint8_t count) {
|
||
const uint8_t SYNC_WRITE = 0x83;
|
||
const uint8_t START_BYTE = 0xFF;
|
||
const uint8_t BROADCAST_ID = 0xFE;
|
||
const uint8_t START_ADDR = 0x2A; // Goal Position address
|
||
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++] = START_BYTE;
|
||
packet[index++] = START_BYTE;
|
||
packet[index++] = BROADCAST_ID;
|
||
packet[index++] = packetLen;
|
||
packet[index++] = SYNC_WRITE;
|
||
packet[index++] = START_ADDR;
|
||
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 a ping command to a specific servo
|
||
void 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();
|
||
Serial1.write(packet, sizeof(packet));
|
||
Serial1.flush();
|
||
SetModeReceive();
|
||
|
||
if (PRINT_SENT_PACKETS) {
|
||
Serial.print("Sent ping packet: ");
|
||
for (int i = 0; i < 6; i++) {
|
||
Serial.print("0x");
|
||
if (packet[i] < 0x10) Serial.print("0"); // Leading zero for single-digit hex
|
||
Serial.print(packet[i], HEX);
|
||
Serial.print(" ");
|
||
}
|
||
Serial.println();
|
||
}
|
||
}
|
||
|
||
void WaitOnReply(unsigned long timeout) {
|
||
unsigned long startTime = millis(); // Record the start time
|
||
|
||
while (millis() - startTime < timeout) { // Loop until timeout
|
||
if (Serial1.available()) {
|
||
uint8_t buffer[32];
|
||
int count = 0;
|
||
|
||
// Read all available bytes
|
||
while (Serial1.available() && count < sizeof(buffer)) {
|
||
buffer[count++] = Serial1.read();
|
||
}
|
||
|
||
// 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
|
||
}
|
||
}
|
||
|
||
void SetModeTransmit() {
|
||
digitalWrite(DE_PIN, HIGH);
|
||
digitalWrite(RE_PIN, HIGH);
|
||
delay(10);
|
||
}
|
||
|
||
void SetModeReceive() {
|
||
digitalWrite(DE_PIN, LOW);
|
||
digitalWrite(RE_PIN, LOW);
|
||
delay(10);
|
||
}
|
||
|
||
void PingAll() {
|
||
Serial.println("PINGING ALL 0-255");
|
||
for (int i = 180; i < 255; i++) {
|
||
|
||
sendPing(i);
|
||
|
||
WaitOnReply(100);
|
||
}
|
||
Serial.println("PINGING COMPLETE");
|
||
}
|
||
|
||
void TestRange(uint8_t id) {
|
||
for (int i = 0; i < 4095; i += 256) {
|
||
sendWritePos(id, i);
|
||
WaitOnReply(100);
|
||
delay(100);
|
||
}
|
||
for (int i = 4095; i > 0; i -= 256) {
|
||
|
||
sendWritePos(id, i);
|
||
WaitOnReply(100);
|
||
delay(100);
|
||
}
|
||
}
|
||
|
||
uint8_t ids[] = { 1, 2 }; //, 10, 15, 40, 41, 43, 44};
|
||
uint16_t pos[2];
|
||
|
||
void loop() {
|
||
// Serial.println(servos.getModel(1));
|
||
uint16_t pos = servos.getModel(1);
|
||
uint16_t swapped = (pos >> 8) | (pos << 8);
|
||
Serial.println(swapped);
|
||
delay(1000);
|
||
return;
|
||
servos.sendWritePos(1, 0);
|
||
for (int i = 0; i < 100; i++) {
|
||
uint16_t pos = servos.getPosition(1);
|
||
uint16_t swapped = (pos >> 8) | (pos << 8);
|
||
Serial.println(swapped);
|
||
delay(10);
|
||
}
|
||
delay(1000);
|
||
servos.sendWritePos(1, 1023);
|
||
for (int i = 0; i < 100; i++) {
|
||
uint16_t pos = servos.getPosition(1);
|
||
uint16_t swapped = (pos >> 8) | (pos << 8);
|
||
Serial.println(swapped);
|
||
delay(10);
|
||
}
|
||
delay(1000);
|
||
//servos.pingAll();
|
||
//servos.waitOnReply(50);
|
||
delay(1000);
|
||
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);
|
||
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);
|
||
Serial.println(servos.getPosition(103));
|
||
//Serial.println(i);
|
||
delay(2000);
|
||
}
|
||
}
|
||
|
||
//servos.testRequest();
|
||
//servos.update();
|
||
//servos.enableTorque(1);
|
||
//servos.enableTorque(2);
|
||
// servos.pingAll();
|
||
// delay(2000);
|
||
// for (int i = 0; i < 4096; i += 32) {
|
||
// servos.sendWritePos(103, i);
|
||
// servos.waitOnReply(10);
|
||
// delay(100);
|
||
// }
|
||
return;
|
||
//servos.update();
|
||
// //servos.pingAll();
|
||
|
||
// //servos.sendWritePos(1, 100);
|
||
//delay(2000);
|
||
// servos.waitOnReply(10);
|
||
// delay(2000);
|
||
// servos.sendRequest(103, 0x02);
|
||
// servos.waitOnReply(10);
|
||
// delay(2000);
|
||
// for (int i = 0; i < 1024; i++){
|
||
// servos.sendWritePos(103, i);
|
||
// servos.waitOnReply(10);
|
||
// Serial.println(i);
|
||
// delay(50);
|
||
// }
|
||
|
||
// servos.waitOnReply(10);
|
||
// delay(2000);
|
||
// servos.sendWritePos(103, 500);
|
||
// servos.waitOnReply(10);
|
||
// delay(2000);
|
||
// servos.sendWritePos(103, 1000);
|
||
// //servos.sendRequest(103, 0x02);
|
||
// servos.waitOnReply(10);
|
||
// delay(2000);
|
||
return;
|
||
|
||
//WaitOnReply(100);
|
||
//PingAll();
|
||
//return;
|
||
// for (uint16_t pos = 0; pos <= 1024; pos += 16) {
|
||
// syncWriteTwoServos(pos, 1024 - pos);
|
||
// delay(500);
|
||
// }
|
||
|
||
// for (int i = 0; i < 1024; i += 8) {
|
||
// for (int x = 0; x < 2; x++) {
|
||
// pos[x] = i;
|
||
// }
|
||
// //sendWritePos(1, i);
|
||
// sendWritePos(103, i);
|
||
// //syncWriteServos(ids, pos, 2);
|
||
// Serial.println(i);
|
||
// //syncWriteTwoServos(ids[0], pos[0], ids[1], pos[1]);
|
||
// delay(10);
|
||
// }
|
||
|
||
// for (int i = 1025; i > 0; i -= 8) {
|
||
// for (int x = 0; x < 2; x++) {
|
||
// pos[x] = i;
|
||
// }
|
||
// //sendWritePos(1, i);
|
||
// sendWritePos(103, i);
|
||
// //syncWriteServos(ids, pos, 2);
|
||
// Serial.println(i);
|
||
// //syncWriteTwoServos(ids[0], pos[0], ids[1], pos[1]);
|
||
// delay(10);
|
||
// }
|
||
|
||
|
||
//TestRange(1);
|
||
// TestRange(4);
|
||
// TestRange(10);
|
||
// TestRange(15);
|
||
// TestRange(40);
|
||
// TestRange(41);
|
||
// TestRange(43);
|
||
// TestRange(44);
|
||
|
||
// sendWritePos(123, 1000);
|
||
//delay(2000);
|
||
|
||
// sendWritePos(123, 2000);
|
||
// delay(2000);
|
||
|
||
|
||
|
||
// sendWritePos(34, 4095);
|
||
// WaitOnReply(50);
|
||
// delay(100);
|
||
// sendWritePos(34, 0);
|
||
// WaitOnReply(50);
|
||
// delay(100);
|
||
|
||
// WaitOnReply(1000);
|
||
// delay(1000);
|
||
// delay(1000);
|
||
// sendPing(5);
|
||
// Serial.println("WAITING...");
|
||
// WaitOnReply(1000);
|
||
// delay(1000);
|
||
} |