66 lines
1.4 KiB
C++
66 lines
1.4 KiB
C++
#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();
|
|
}
|