realrobotshk
/
HansonServo
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
HansonServo/HansonServo.ino

892 lines
23 KiB
C++
Raw Blame History

#include <base64.h>
#include "feetech.h"
#include "animation.h"
#define DEVICE_NAME "Little Sophia"
#define FIRMWARE_VERSION "0.0.1"
#define HEADER1 0xAA
#define HEADER2 0x55
#define MAX_PAYLOAD_SIZE 6000 // 10 KB
uint8_t payload[MAX_PAYLOAD_SIZE]; // Global or static
#define CMD_ID_REQUEST 0x01
#define CMD_FILE_LIST 0x02
#define CMD_LOAD_FILE 0x03
#define CMD_DELETE_FILE 0x04
#define CMD_SAVE_FILE 0x05
#define CMD_MESSAGE 0x06
#define CMD_SET_POSITION 0x07
#define CMD_PLAY_FILE 0x08
#define CMD_SCAN_CHANNEL 0x09
// ESP32 S2 PINOUT
#define CH0_RX_PIN 13
#define CH0_TX_PIN 12
// #define RX_PIN 18 // RO
// #define TX_PIN 17 // DI
#define CH1_RX_PIN 11 // RO
#define CH1_TX_PIN 10 // DI
#define DE_PIN 7 // Driver Enable
#define RE_PIN 8 // Receiver Enable
Animation anim;
Animation sweep;
Animation stagger;
Feetech* servos[2];
uint16_t flipBytes(uint16_t value) {
return (value >> 8) | (value << 8);
}
uint8_t ids[NUM_CHANNELS] = { 10, 11, 12, 13, 14 };
uint16_t pos1[] = { 0, 0, 0, 0, 0 };
uint16_t pos2[] = { 1023, 1023, 1023, 1023, 1023 };
uint8_t idsSTS[NUM_CHANNELS] = { 15, 103 };
uint16_t pos1STS[] = { 0, 0 };
uint16_t pos2STS[] = { 1023, 1023 };
void setup() {
Serial.begin(1000000);
for (int i = 0; i < 5; i++) {
Serial.println(i);
delay(500);
}
servos[0] = new Feetech(Serial1, DE_PIN, RE_PIN, CH0_TX_PIN, CH0_RX_PIN); // SCS
servos[0]->setFeetechMode(Feetech::MODE_SCS);
servos[1] = new Feetech(Serial2, DE_PIN, RE_PIN, CH1_TX_PIN, CH1_RX_PIN); // STS
servos[1]->setFeetechMode(Feetech::MODE_STS);
// pinMode(RX_PIN, OUTPUT);
// pinMode(TX_PIN, OUTPUT);
// pinMode(DE_PIN, OUTPUT);
// while (true){
// Serial.println(!digitalRead(RX_PIN));
// digitalWrite(RX_PIN, !digitalRead(RX_PIN));
// digitalWrite(TX_PIN, !digitalRead(TX_PIN));
// digitalWrite(DE_PIN, !digitalRead(DE_PIN));
// delay(2000);
// }
pos2[3] = flipBytes(pos2[3]);
servos[0]->begin();
servos[1]->begin();
if (!FFat.begin(true)) {
Serial.println("FFat mount failed");
return;
}
// anim.loadFromFile("/bob.anim");
// anim.printKeyframes();
// playAnimation(anim);
// ClearFiles();
// // PrintFileList();
// sweep.clear();
// sweep.createSampleSweep(4);
// sweep.saveToFile("/sweep.anim");
// stagger.clear();
// stagger.createStaggeredSweep(4);
// stagger.saveToFile("/stagger.anim");
// //delay(9999);
// sweep.printKeyframes();
// anim.clear();
// if (!sweep.loadFromFile("/sweep.anim")) {
// Serial.println("Failed to load animation");
// return;
// } else {
// Serial.println("Loaded sweep anim");
// }
// if (!stagger.loadFromFile("/stagger.anim")) {
// Serial.println("Failed to load animation");
// return;
// } else {
// Serial.println("Loaded stagger anim");
// }
// Serial.println(anim.getFrame(0, 0)); // Should show saved value
//SetID(12, 16);
//Serial.println(servos[1]->setLockSTS(16, 0));
// Serial.println("Enable torque");
// Serial.println(servos[0]->enableTorque(13));
// Serial.println(servos[1]->enableTorque(13));
// delay(3000);
// Serial.println("Disable torque");
// Serial.println(servos[0]->disableTorque(13));
// Serial.println(servos[1]->disableTorque(13));
// delay(3000);
// servos[0]->setMinAngleLimit(13, 100);
// servos[1]->setMinAngleLimit(13, 900);
// servos[0]->setMaxAngleLimit(13, 500);
// servos[1]->setMaxAngleLimit(13, 3900);
}
void SetID(uint8_t oldID, uint8_t newID) {
Serial.println("Setting Lock to 0");
Serial.println(servos[0]->setLockSTS(oldID, 0));
delay(1000);
Serial.print("Changing ID ");
Serial.print(oldID);
Serial.print(" to ");
Serial.println(newID);
Serial.println(servos[0]->setID(oldID, newID));
delay(1000);
Serial.println("Setting Lock to 1");
Serial.println(servos[0]->setLockSTS(newID, 1));
delay(1000);
}
void HandleSerialRequests() {
if (Serial.available() >= 6) { // 2 headers + 1 command + 2 length + 1 checksum minimum
if (Serial.read() == HEADER1 && Serial.read() == HEADER2) {
uint8_t command = Serial.read();
uint16_t length = (Serial.read() << 8) | Serial.read();
if (length > MAX_PAYLOAD_SIZE) {
Serial.println("Payload too large");
while (Serial.available()) Serial.read(); // flush junk
return;
}
unsigned long start = millis();
while (Serial.available() < length + 1) {
if (millis() - start > 1000) { // 1 second timeout
Serial.println("Serial timeout");
return;
}
}
uint8_t checksum = command ^ (length >> 8) ^ (length & 0xFF);
for (uint16_t i = 0; i < length; i++) {
payload[i] = Serial.read();
checksum ^= payload[i];
}
uint8_t receivedChecksum = Serial.read();
if (checksum == receivedChecksum) {
handleCommand(command, payload, length);
} else {
Serial.println("Checksum mismatch");
}
}
}
}
void sendMessage(const String& payload, uint8_t command = CMD_MESSAGE) {
uint16_t length = payload.length();
uint8_t checksum = CMD_MESSAGE ^ (length >> 8) ^ (length & 0xFF);
for (int i = 0; i < length; i++) {
checksum ^= payload[i];
}
Serial.write(HEADER1);
Serial.write(HEADER2);
Serial.write(command);
Serial.write((length >> 8) & 0xFF);
Serial.write(length & 0xFF);
Serial.write((const uint8_t*)payload.c_str(), length);
Serial.write(checksum);
}
void sendMessage(const uint8_t* payload, uint16_t length, uint8_t command = CMD_MESSAGE) {
uint8_t checksum = command ^ (length >> 8) ^ (length & 0xFF);
for (uint16_t i = 0; i < length; i++) {
checksum ^= payload[i];
}
Serial.write(HEADER1);
Serial.write(HEADER2);
Serial.write(command);
Serial.write((length >> 8) & 0xFF);
Serial.write(length & 0xFF);
Serial.write(payload, length);
Serial.write(checksum);
}
void handleCommand(uint8_t command, const uint8_t* payload, uint16_t length) {
switch (command) {
case CMD_ID_REQUEST: // CMD_ID_REQUEST
handleIdRequest();
break;
case CMD_FILE_LIST: //
handleFileList();
break;
case CMD_LOAD_FILE: //
handleLoadFile(payload, length);
break;
case CMD_DELETE_FILE: //
handleDeleteFile(payload, length);
break;
case CMD_SAVE_FILE:
handleSaveFile(payload, length);
break;
case CMD_SET_POSITION:
handleSetPosition(payload, length);
break;
case CMD_PLAY_FILE:
handlePlayAnimation(payload, length);
break;
case CMD_SCAN_CHANNEL:
handleScanChannel(payload, length);
break;
default:
Serial.print("Unknown command: ");
Serial.println(command, HEX);
break;
}
}
void handleIdRequest() {
String payload = String(DEVICE_NAME) + "|" + FIRMWARE_VERSION;
uint16_t length = payload.length();
uint8_t checksum = CMD_ID_REQUEST ^ (length >> 8) ^ (length & 0xFF);
for (int i = 0; i < length; i++) {
checksum ^= payload[i];
}
Serial.write(HEADER1);
Serial.write(HEADER2);
Serial.write(CMD_ID_REQUEST);
Serial.write((length >> 8) & 0xFF);
Serial.write(length & 0xFF);
Serial.write((const uint8_t*)payload.c_str(), length);
Serial.write(checksum);
}
void handleFileList() {
File root = FFat.open("/");
if (!root || !root.isDirectory()) {
sendFileListResponse(""); // empty payload
return;
}
String payload = "";
File file = root.openNextFile();
while (file) {
if (!file.isDirectory()) {
payload += String(file.name()) + "\n";
}
file = root.openNextFile();
}
sendFileListResponse(payload);
}
void sendFileListResponse(const String& payloadStr) {
uint16_t length = payloadStr.length();
if (length > MAX_PAYLOAD_SIZE) {
Serial.println("File list too large");
return;
}
uint8_t checksum = CMD_FILE_LIST ^ (length >> 8) ^ (length & 0xFF);
for (uint16_t i = 0; i < length; i++) {
checksum ^= payloadStr[i];
}
Serial.write(HEADER1);
Serial.write(HEADER2);
Serial.write(CMD_FILE_LIST);
Serial.write((length >> 8) & 0xFF);
Serial.write(length & 0xFF);
Serial.write((const uint8_t*)payloadStr.c_str(), length);
Serial.write(checksum);
}
void handleLoadFile(const uint8_t* payload, uint16_t length) {
if (length == 0 || length >= 128) {
Serial.println("Invalid filename");
return;
}
char filename[128];
memcpy(filename, payload, length);
filename[length] = '\0';
File file = FFat.open(filename, "r");
if (!file || !file.available()) {
Serial.println("File not found or empty");
return;
}
size_t fileSize = file.size();
if (fileSize > 65535) {
Serial.println("File too large for single transfer");
file.close();
return;
}
uint8_t* buffer = (uint8_t*)malloc(fileSize);
if (!buffer) {
Serial.println("Memory allocation failed");
file.close();
return;
}
size_t bytesRead = file.read(buffer, fileSize);
file.close();
if (bytesRead != fileSize) {
Serial.println("File read error");
free(buffer);
return;
}
// 🔹 Compute checksum
uint8_t checksum = CMD_LOAD_FILE ^ (fileSize >> 8) ^ (fileSize & 0xFF);
for (size_t i = 0; i < fileSize; i++) {
checksum ^= buffer[i];
}
// 🔹 Send packet
Serial.write(HEADER1);
Serial.write(HEADER2);
Serial.write(CMD_LOAD_FILE);
Serial.write((fileSize >> 8) & 0xFF);
Serial.write(fileSize & 0xFF);
Serial.write(buffer, fileSize);
Serial.write(checksum);
free(buffer);
Serial.println("File sent in one go");
}
void handleDeleteFile(const uint8_t* payload, uint16_t length) {
sendMessage("Deleting FILE");
if (length < 1) {
Serial.println("Payload too short for filename length");
sendMessage("Payload too short for filename length");
return;
}
// 🔹 Parse filename
uint16_t filenameLength = payload[0] | (payload[1] << 8);
if (length < 2 + filenameLength) {
Serial.println("Payload too short for filename");
sendMessage("Payload too short for filename");
return;
}
char filename[filenameLength + 1];
memcpy(filename, payload + 2, filenameLength);
filename[filenameLength] = '\0';
deleteFile(FFat, ("/" + String(filename)).c_str());
sendMessage(("File Deleted: " + String(filename)).c_str(), CMD_DELETE_FILE);
}
void handlePlayAnimation(const uint8_t* payload, uint16_t length) {
sendMessage("Playing FILE");
if (length < 1) {
Serial.println("Payload too short for filename length");
sendMessage("Payload too short for filename length");
return;
}
// 🔹 Parse filename
uint16_t filenameLength = payload[0] | (payload[1] << 8);
if (length < 2 + filenameLength) {
Serial.println("Payload too short for filename");
sendMessage("Payload too short for filename");
return;
}
char filename[filenameLength + 1];
memcpy(filename, payload + 2, filenameLength);
filename[filenameLength] = '\0';
//deleteFile(FFat, ("/" + String(filename)).c_str());
anim.clear();
anim.loadFromFile(("/" + String(filename)).c_str());
playAnimation(anim);
sendMessage("File Played", CMD_PLAY_FILE);
}
void handleSaveFile(const uint8_t* payload, uint16_t length) {
bool valid = parseAnimationPayload(payload, length, anim);
if (valid) {
//Serial.println("Animation parsed successfully!");
//anim.printKeyframes();
} else {
//Serial.println("Failed to parse animation.");
length = 1; // Override length to 1 for fallback response
payload = nullptr; // We'll send a single 0x00 instead
}
if (length > MAX_PAYLOAD_SIZE) {
Serial.println("File list too large");
return;
}
// Calculate checksum
uint8_t checksum = CMD_SAVE_FILE ^ (length >> 8) ^ (length & 0xFF);
if (valid) {
for (uint16_t i = 0; i < length; i++) {
checksum ^= payload[i];
}
} else {
checksum ^= 0x00;
}
// Send response
Serial.write(HEADER1);
Serial.write(HEADER2);
Serial.write(CMD_SAVE_FILE);
Serial.write((length >> 8) & 0xFF);
Serial.write(length & 0xFF);
if (valid) {
for (uint16_t i = 0; i < length; i++) {
Serial.write(payload[i]);
}
} else {
Serial.write(0x00);
}
Serial.write(checksum);
}
bool parseAnimationPayload(const uint8_t* payload, uint16_t length, Animation& animation) {
sendMessage("SAVING FILE");
if (length < 1) {
Serial.println("Payload too short for filename length");
return false;
}
// 🔹 Parse filename
uint16_t filenameLength = payload[0] | (payload[1] << 8);
if (length < 2 + filenameLength + 18) {
Serial.println("Payload too short for filename and header");
return false;
}
char filename[filenameLength + 1];
memcpy(filename, payload + 2, filenameLength);
filename[filenameLength] = '\0';
const uint8_t* ptr = payload + 2 + filenameLength;
String msg = "SAVING FILE: ";
msg += filename;
sendMessage(msg);
// 🔹 Parse header
memcpy(animation.header.magic, ptr, 4);
if (strncmp(animation.header.magic, "ANIM", 4) != 0) {
Serial.println("Invalid magic header");
sendMessage("invalid magic header");
return false;
}
animation.header.frameCount = (ptr[4] << 8) | ptr[5];
animation.header.version = ptr[6];
animation.header.frameRate = ptr[7];
memcpy(animation.header.reserved, ptr + 8, 8);
uint16_t keyframeCount = ptr[16] | (ptr[17] << 8);
ptr += 18;
if (length < (ptr - payload) + keyframeCount * 5) {
Serial.println("Payload too short for keyframes");
sendMessage("Payload too short");
return false;
}
// 🔹 Parse keyframes
animation.clear();
for (uint16_t i = 0; i < keyframeCount; i++) {
uint8_t motorId = ptr[0];
uint16_t frame = ptr[1] | (ptr[2] << 8);
uint16_t position = ptr[3] | (ptr[4] << 8);
animation.addKeyframe(motorId, frame, position);
ptr += 5;
}
// 🔹 Save using received filename
animation.saveToFile(("/" + String(filename)).c_str());
sendMessage("SAVED");
return true;
}
void handleSetPosition(const uint8_t* payload, uint16_t length) {
if (length % 3 != 0) {
Serial.println("Invalid sync packet length");
return;
}
uint8_t count = 0;
for (int i = 0; i < length; i += 3) {
uint8_t motorId = payload[i];
uint16_t position = (payload[i + 2] << 8) | payload[i + 1];
pos1[count] = position;
count++;
// Apply position to motorId
//servos.sendWritePos(ids[motorId], position);
}
servos[0]->syncWritePos(ids, pos1, NUM_CHANNELS);
}
// Scans 0-254 and responds with the channel and ID as each successful ping is received
// Signals end by responding with channel and 255
void handleScanChannel(const uint8_t* payload, uint16_t length) {
if (length != 1) {
sendMessage("Length of scanChannel Request Wrong");
return;
}
for (int i = 0; i < 254; i++) {
servos[payload[0]]->sendPing(i);
uint8_t val = servos[payload[0]]->waitOnData1Byte(10);
if (val != 0) {
uint8_t response[4];
response[0] = payload[0]; // channel
response[1] = i; // responding address
uint16_t mode = servos[payload[0]]->getModel(i);
uint16_t minAngleLimit = servos[payload[0]]->getMinAngleLimit(i);
uint16_t maxAngleLimit = servos[payload[0]]->getMaxAngleLimit(i);
uint16_t position = servos[payload[0]]->getPosition(i);
response[2] = (mode >> 8) & 0xFF; // high byte
response[3] = mode & 0xFF; // low byte
response[4] = (minAngleLimit >> 8) & 0xFF;
response[5] = minAngleLimit & 0xFF;
response[6] = (maxAngleLimit >> 8) & 0xFF;
response[7] = maxAngleLimit & 0xFF;
response[8] = (position >> 8) & 0xFF;
response[9] = position & 0xFF;
sendMessage(response, 10, CMD_SCAN_CHANNEL); // send all 4 bytes
}
}
uint8_t r[2];
r[0] = payload[0]; // channel
r[1] = 255; // responding address
sendMessage(r, 2, CMD_SCAN_CHANNEL);
// std::vector<uint8_t> successfulAddresses;
// servos[payload[0]]->pingAll(successfulAddresses);
// std::vector<uint8_t> response;
// response.push_back(payload[0]); // channel
// response.push_back(successfulAddresses.size()); // count
// for (uint8_t address : successfulAddresses) {
// response.push_back(address);
// }
// sendMessage(response.data(), response.size(), CMD_SCAN_CHANNEL);
}
bool flip = false;
unsigned long lastSend = 0;
void loop() {
HandleSerialRequests();
// for (int i = 0; i < 500; i+=32) {
// // Serial.print("WRITE: ");
// // Serial.print(i);
// // Serial.print(" ");
// // Serial.println(servos[1]->setMinAngleLimit(16, i));
// Serial.print("READ: ");
// Serial.println(servos[1]->getMinAngleLimit(16));
// delay(2000);
// }
// servos[0]->sendWritePos(13, 0);
// servos[1]->sendWritePos(13, 0);
// Serial.print(servos[0]->getMinAngleLimit(13));
// Serial.print("\t");
// Serial.print(servos[0]->getMaxAngleLimit(13));
// Serial.print("\t");
// Serial.print(servos[1]->getMinAngleLimit(13));
// Serial.print("\t");
// Serial.println(servos[1]->getMaxAngleLimit(13));
// delay(1000);
// //
// for (int i = 0; i < 4095; i+=16){
// Serial.println(i);
// servos[0]->sendWritePos(13, i);
// servos[1]->sendWritePos(13, i);
// delay(20);
// }
// delay(1000);
// for (int i = 4095; i > 0; i-=16){
// Serial.println(i);
// servos[0]->sendWritePos(13, i);
// servos[1]->sendWritePos(13, i);
// delay(20);
// }
// delay(1000);
// delay(1000);
// for (int i = 0; i< 100; i++){
// Serial.println(servos[1]->getGoalSpeed(13));
// delay(10);
// }
// servos[0]->sendWritePos(13, 0);
// for (int i = 0; i< 100; i++){
// Serial.println(servos[1]->getGoalSpeed(13));
// delay(10);
// }
if (millis() - lastSend > 2000) {
//sendMessageFromESP32(String(millis()));
//handleIdRequest();
//PrintFileList();
lastSend = millis();
}
}
void PrintFileList() {
File root = FFat.open("/");
if (!root || !root.isDirectory()) {
Serial.println("Failed to open FFat root directory");
return;
}
Serial.println("Files in FFat:");
File file = root.openNextFile();
while (file) {
Serial.print(" ");
Serial.print(file.name());
Serial.print(" | Size: ");
Serial.println(file.size());
file = root.openNextFile();
}
Serial.println("End of file list.");
}
void ClearFiles() {
File root = FFat.open("/");
if (!root || !root.isDirectory()) {
Serial.println("Failed to open FFat root directory");
return;
}
Serial.println("Files in FFat:");
File file = root.openNextFile();
while (file) {
String filename = "/" + String(file.name()); // Add leading slash
Serial.print(" Deleting: ");
Serial.println(filename);
file.close(); // Close before deleting
deleteFile(FFat, filename.c_str()); // Use corrected path
file = root.openNextFile();
}
Serial.println("FFat cleanup complete.");
}
void deleteFile(fs::FS& fs, const char* path) {
Serial.printf("Deleting file: %s\r\n", path);
if (fs.remove(path)) {
Serial.println("- file deleted");
} else {
Serial.println("- delete failed");
}
}
void playAnimation(Animation& anim) {
uint16_t positions[NUM_CHANNELS];
const uint16_t frameCount = 400; //anim.getFrameCount();
const uint32_t frameDelay = 1000 / FRAMES_PER_SECOND;
uint32_t nextFrameTime = millis();
Serial.print("Frame Count: ");
Serial.println(frameCount);
// Organize keyframes per motor
std::vector<Keyframe> motorKeyframes[NUM_CHANNELS];
for (const auto& kf : anim.getKeyframes()) {
if (kf.motorId < NUM_CHANNELS) {
motorKeyframes[kf.motorId].push_back(kf);
}
}
// Sort keyframes per motor by frame
for (int ch = 0; ch < NUM_CHANNELS; ch++) {
std::sort(motorKeyframes[ch].begin(), motorKeyframes[ch].end(),
[](const Keyframe& a, const Keyframe& b) {
return a.frame < b.frame;
});
}
for (uint16_t frame = 0; frame < frameCount; frame++) {
while (millis() < nextFrameTime) {
delay(1);
}
for (int ch = 0; ch < NUM_CHANNELS; ch++) {
const auto& kfs = motorKeyframes[ch];
uint16_t value = 512; // default position
// Find surrounding keyframes
Keyframe prev = { ch, 0, 512 }, next = { ch, frameCount, 512 };
for (size_t i = 0; i < kfs.size(); i++) {
if (kfs[i].frame <= frame) {
prev = kfs[i];
}
if (kfs[i].frame > frame) {
next = kfs[i];
break;
}
}
// Interpolate
if (prev.frame == next.frame) {
value = prev.position;
} else {
float t = float(frame - prev.frame) / (next.frame - prev.frame);
value = prev.position + t * (next.position - prev.position);
}
positions[ch] = value;
}
Serial.println(positions[0]);
servos[0]->syncWritePos(ids, positions, NUM_CHANNELS);
nextFrameTime += frameDelay;
}
}
void playAnimationOLD(Animation& anim) {
uint16_t positions[NUM_CHANNELS];
const uint16_t frameCount = anim.getFrameCount();
const uint32_t frameDelay = 1000 / FRAMES_PER_SECOND; // 20 ms
uint32_t nextFrameTime = millis();
for (uint16_t frame = 0; frame < frameCount; frame++) {
// Wait until it's time for the next frame
while (millis() < nextFrameTime) {
// Optional: yield or do background tasks here
delay(1);
}
// Send frame to servos
if (anim.getFramePositions(frame, positions)) {
servos[0]->syncWritePos(ids, positions, NUM_CHANNELS);
}
// Schedule next frame
nextFrameTime += frameDelay;
}
}
void playLayeredAnimation(Animation& base, Animation& overlay) {
uint16_t basePositions[NUM_CHANNELS];
uint16_t overlayPositions[NUM_CHANNELS];
uint16_t finalPositions[NUM_CHANNELS];
const uint16_t frameCount = base.getFrameCount();
const uint32_t frameDelay = 1000 / FRAMES_PER_SECOND;
uint32_t nextFrameTime = millis();
for (uint16_t frame = 0; frame < frameCount; frame++) {
while (millis() < nextFrameTime) delay(1);
base.getFramePositions(frame, basePositions);
overlay.getFramePositions(frame, overlayPositions);
for (uint8_t ch = 0; ch < NUM_CHANNELS; ch++) {
finalPositions[ch] = (basePositions[ch] + overlayPositions[ch]) / 2;
}
servos[0]->syncWritePos(ids, finalPositions, NUM_CHANNELS);
nextFrameTime += frameDelay;
}
}
void SCSPingAll() {
std::vector<uint8_t> successfulAddresses;
servos[0]->pingAll(successfulAddresses);
// Now successfulAddresses contains all successful pings
Serial.println("Successful Addresses:");
for (uint8_t address : successfulAddresses) {
Serial.print(address);
Serial.print(" ");
}
Serial.println();
}
void STSPingAll() {
std::vector<uint8_t> successfulAddresses;
servos[1]->pingAll(successfulAddresses);
// Now successfulAddresses contains all successful pings
Serial.println("Successful Addresses:");
for (uint8_t address : successfulAddresses) {
Serial.print(address);
Serial.print(" ");
}
Serial.println();
}
Reference in New Issue View Git Blame Copy Permalink