realrobotshk
/
rs485transmitter
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

syncwrite and id changes for STS/SCS

master
Jake 2025-09-22 02:15:17 +08:00
parent 5b468195dc
commit 7611b4509c
3 changed files with 385 additions and 451 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

146
feetech.cpp
View File

@ -7,10 +7,54 @@ Feetech::Feetech(HardwareSerial& serial, int DE_PIN, int RE_PIN, int TX_PIN, int
void Feetech::begin() { void Feetech::begin() {
serial.begin(1000000, SERIAL_8N1, RX_PIN, TX_PIN); serial.begin(1000000, SERIAL_8N1, RX_PIN, TX_PIN);
pinMode(DE_PIN, OUTPUT); pinMode(DE_PIN, OUTPUT);
pinMode(RE_PIN, OUTPUT); //pinMode(RE_PIN, OUTPUT);
setModeReceive(); 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 // Send move command to servo id:0-255, position:0-4095
void Feetech::sendWritePos(uint8_t id, uint16_t position) { void Feetech::sendWritePos(uint8_t id, uint16_t position) {
uint8_t packet[9]; uint8_t packet[9];
@ -51,9 +95,12 @@ void Feetech::sendPing(uint8_t id) {
packet[5] = ~sum; // Checksum packet[5] = ~sum; // Checksum
// Send packet // Send packet
//Serial.println("PING");
setModeTransmit(); setModeTransmit();
//delay(20);
serial.write(packet, sizeof(packet)); serial.write(packet, sizeof(packet));
serial.flush(); serial.flush();
//delay(20);
setModeReceive(); setModeReceive();
} }
@ -104,6 +151,11 @@ uint8_t Feetech::getID(uint8_t id) {
return waitOnData1Byte(10); 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) { uint8_t Feetech::getBaudRate(uint8_t id) {
sendRequest(id, REQUEST_BAUD_RATE, 1); sendRequest(id, REQUEST_BAUD_RATE, 1);
return waitOnData1Byte(10); return waitOnData1Byte(10);
@ -165,7 +217,17 @@ uint16_t Feetech::getGoalSpeed(uint8_t id) {
} }
uint8_t Feetech::getLock(uint8_t id) { uint8_t Feetech::getLock(uint8_t id) {
sendRequest(id, REQUEST_TORQUE_ENABLE, 1); 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); return waitOnData1Byte(10);
} }
@ -183,7 +245,7 @@ uint16_t Feetech::getPosition(uint8_t id) {
int16_t Feetech::getSpeed(uint8_t id) { int16_t Feetech::getSpeed(uint8_t id) {
sendRequest(id, REQUEST_CURRENT_SPEED, 2); sendRequest(id, REQUEST_CURRENT_SPEED, 2);
int16_t val = waitOnData2Bytes(10); int16_t val = waitOnData2Bytes(10);
if (val < 0){ if (val < 0) {
val -= 32767; val -= 32767;
val = -val; val = -val;
} }
@ -221,7 +283,7 @@ void Feetech::sendRequest(uint8_t id, byte instruction, uint8_t byteCount) {
packet[3] = 0x04; // Length (4 bytes of following data) packet[3] = 0x04; // Length (4 bytes of following data)
packet[4] = READ_DATA; // Instruction packet[4] = READ_DATA; // Instruction
packet[5] = instruction; // Write first address packet[5] = instruction; // Write first address
packet[6] = byteCount; // Number of bytes to read packet[6] = byteCount; // Number of bytes to read
// Calculate checksum // Calculate checksum
uint8_t sum = 0; uint8_t sum = 0;
@ -234,12 +296,62 @@ void Feetech::sendRequest(uint8_t id, byte instruction, uint8_t byteCount) {
setModeReceive(); setModeReceive();
} }
void Feetech::pingAll() { void Feetech::write1Byte(uint8_t id, byte instruction, uint8_t data) {
Serial.println("PINGING ALL 0-255"); uint8_t packet[8];
for (int i = 0; i < 255; i++) {
sendPing(i);
waitOnReply(50); 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"); Serial.println("PINGING COMPLETE");
} }
@ -249,7 +361,7 @@ void Feetech::waitOnReply(unsigned long timeout) {
while (millis() - startTime < timeout) { // Loop until timeout while (millis() - startTime < timeout) { // Loop until timeout
if (serial.available()) { if (serial.available()) {
Serial.println("RECV"); //Serial.println("RECV");
uint8_t buffer[32]; uint8_t buffer[32];
int count = 0; int count = 0;
@ -258,7 +370,7 @@ void Feetech::waitOnReply(unsigned long timeout) {
buffer[count++] = serial.read(); buffer[count++] = serial.read();
} }
Serial.println(count); //Serial.println(count);
// Display on Serial // Display on Serial
Serial.print("recv: "); Serial.print("recv: ");
@ -290,7 +402,7 @@ uint8_t Feetech::waitOnData1Byte(unsigned long timeout) {
while (millis() - startTime < timeout) { // Loop until timeout while (millis() - startTime < timeout) { // Loop until timeout
if (serial.available()) { if (serial.available()) {
Serial.println("RECV"); //Serial.println("RECV");
uint8_t buffer[32]; uint8_t buffer[32];
int count = 0; int count = 0;
@ -299,7 +411,7 @@ uint8_t Feetech::waitOnData1Byte(unsigned long timeout) {
buffer[count++] = serial.read(); buffer[count++] = serial.read();
} }
Serial.println(count); //Serial.println(count);
// if (count != 8) { // if (count != 8) {
// Serial.print("ERROR: Expected 8 byte reply, recieved "); // Serial.print("ERROR: Expected 8 byte reply, recieved ");
// Serial.println(count); // Serial.println(count);
@ -329,9 +441,12 @@ uint8_t Feetech::waitOnData1Byte(unsigned long timeout) {
return val; return val;
break; // Exit the loop after processing the reply break; // Exit the loop after processing the reply
} else {
//Serial.println(millis() - startTime);
} }
delay(10); // Small delay to prevent busy-waiting delay(1); // Small delay to prevent busy-waiting
} }
return 0;
} }
uint16_t Feetech::waitOnData2Bytes(unsigned long timeout) { uint16_t Feetech::waitOnData2Bytes(unsigned long timeout) {
@ -379,8 +494,9 @@ uint16_t Feetech::waitOnData2Bytes(unsigned long timeout) {
break; // Exit the loop after processing the reply break; // Exit the loop after processing the reply
} }
delay(10); // Small delay to prevent busy-waiting delay(1); // Small delay to prevent busy-waiting
} }
return 0;
} }
void Feetech::sendData(const byte* data, size_t length) { void Feetech::sendData(const byte* data, size_t length) {

22
feetech.h
View File

@ -1,6 +1,7 @@
#ifndef FEETECH_H #ifndef FEETECH_H
#define FEETECH_H #define FEETECH_H
#include <vector>
#include <Arduino.h> #include <Arduino.h>
#include <HardwareSerial.h> #include <HardwareSerial.h>
@ -15,6 +16,7 @@ public:
void sendPing(uint8_t id); void sendPing(uint8_t id);
uint16_t getModel(uint8_t id); uint16_t getModel(uint8_t id);
uint8_t getID(uint8_t id); uint8_t getID(uint8_t id);
uint8_t setID(uint8_t id, uint8_t newId);
uint8_t getBaudRate(uint8_t id); uint8_t getBaudRate(uint8_t id);
uint16_t getMinAngleLimit(uint8_t id); uint16_t getMinAngleLimit(uint8_t id);
uint16_t getMaxAngleLimit(uint8_t id); uint16_t getMaxAngleLimit(uint8_t id);
@ -28,6 +30,8 @@ public:
uint16_t getGoalTime(uint8_t id); uint16_t getGoalTime(uint8_t id);
uint16_t getGoalSpeed(uint8_t id); uint16_t getGoalSpeed(uint8_t id);
uint8_t getLock(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); uint16_t getPosition(uint8_t id);
int16_t getSpeed(uint8_t id); int16_t getSpeed(uint8_t id);
@ -41,7 +45,10 @@ public:
float getVoltage(uint8_t id); float getVoltage(uint8_t id);
void sendRequest(uint8_t id, uint8_t instruction, uint8_t byteCount); void sendRequest(uint8_t id, uint8_t instruction, uint8_t byteCount);
void sendWritePos(uint8_t id, uint16_t position); void sendWritePos(uint8_t id, uint16_t position);
void pingAll(); 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); void waitOnReply(unsigned long timeout);
uint8_t waitOnData1Byte(unsigned long timeout); uint8_t waitOnData1Byte(unsigned long timeout);
uint16_t waitOnData2Bytes(unsigned long timeout); uint16_t waitOnData2Bytes(unsigned long timeout);
@ -60,6 +67,7 @@ public:
static const byte ACTION = 0x05; // TRIGGERS REG WRITE WRITES static const byte ACTION = 0x05; // TRIGGERS REG WRITE WRITES
static const byte SYNCWRITE_DATA = 0x83; // SIMULTANEOUS CONTROL OF MULTIPLE SERVOS static const byte SYNCWRITE_DATA = 0x83; // SIMULTANEOUS CONTROL OF MULTIPLE SERVOS
static const byte RESET = 0x06; // RESET TO FACTORY DEFAULT static const byte RESET = 0x06; // RESET TO FACTORY DEFAULT
static const byte BROADCAST_ID = 0xFE;
// MEMORY TABLE LOCATIONS SMS-STS // MEMORY TABLE LOCATIONS SMS-STS
@ -78,7 +86,7 @@ public:
static const byte REQUEST_GOAL_POSITION = 0x2A; // 2 byte static const byte REQUEST_GOAL_POSITION = 0x2A; // 2 byte
static const byte REQUEST_GOAL_TIME = 0x2C; // 2 byte static const byte REQUEST_GOAL_TIME = 0x2C; // 2 byte
static const byte REQUEST_GOAL_SPEED = 0x2E; // 2 byte static const byte REQUEST_GOAL_SPEED = 0x2E; // 2 byte
static const byte REQUEST_LOCK = 0x37; // 1 byte static const byte REQUEST_LOCK = 0x30; // 1 byte
static const byte REQUEST_POSITION = 0x38; // 2 bytes static const byte REQUEST_POSITION = 0x38; // 2 bytes
@ -91,16 +99,6 @@ public:
static const byte REQUEST_MOVING = 0x42; // 1 byte static const byte REQUEST_MOVING = 0x42; // 1 byte
static const byte REQUEST_CURRENT_CURRENT = 0x45; // 2 bytes static const byte REQUEST_CURRENT_CURRENT = 0x45; // 2 bytes
// TODO
// SMS_STS_PRESENT_SPEED_L = 0x3A
// SMS_STS_PRESENT_SPEED_H = 0x3B
// SMS_STS_PRESENT_LOAD_L = 0x3C
// SMS_STS_PRESENT_LOAD_H = 0x3D
// SMS_STS_PRESENT_VOLTAGE = 0x3E
// SMS_STS_PRESENT_TEMPERATURE = 0x3F
// SMS_STS_MOVING = 0x42
// SMS_STS_PRESENT_CURRENT_L = 0x45
// SMS_STS_PRESENT_CURRENT_H = 0x46
// BAUD RATES (stored as 1 byte) // BAUD RATES (stored as 1 byte)
static const byte SMS_STS_1M = 0; static const byte SMS_STS_1M = 0;

668
rs485transmitter.ino
View File

@ -1,470 +1,290 @@
#include "feetech.h" #include "feetech.h"
#include <WiFi.h>
#include <AsyncTCP.h>
#include <ESPAsyncWebServer.h>
#include <WebSocketsServer.h>
#include <SPIFFS.h>
bool PRINT_SENT_PACKETS = false; // Replace with your network credentials
// S3 setup const char* ssid = "Police Surveillance Van";
// #define RX_PIN 4 // DI const char* password = "ourpassword";
// #define DE_PIN 5 // Driver Enable
// #define RE_PIN 6 // Receiver Enable
// #define TX_PIN 7 // RO
// Create an instance of the server
AsyncWebServer server(80);
WebSocketsServer webSocket = WebSocketsServer(81);
void webSocketTask(void* parameter) {
while (true) {
webSocket.loop(); // Handle WebSocket messages
delay(10); // Small delay to prevent busy-waiting
}
}
void webSocketEvent(uint8_t num, WStype_t type, uint8_t* payload, size_t length) {
switch (type) {
case WStype_DISCONNECTED:
Serial.printf("[%u] Disconnected!\n", num);
break;
case WStype_CONNECTED:
{
IPAddress ip = webSocket.remoteIP(num);
Serial.printf("[%u] Connected from %d.%d.%d.%d url: %s\n", num, ip[0], ip[1], ip[2], ip[3], payload);
// send message to client
//webSocket.sendTXT(num, "Connected");
}
break;
case WStype_TEXT:
Serial.printf("[%u] get Text: %s\n", num, payload);
// send message to client
// webSocket.sendTXT(num, "message here");
// send data to all connected clients
// webSocket.broadcastTXT("message here");
break;
case WStype_BIN:
Serial.printf("[%u] get binary length: %u\n", num, length);
//hexdump(payload, length);
// send message to client
// webSocket.sendBIN(num, payload, length);
break;
case WStype_ERROR:
case WStype_FRAGMENT_TEXT_START:
case WStype_FRAGMENT_BIN_START:
case WStype_FRAGMENT:
case WStype_FRAGMENT_FIN:
break;
}
}
// Function to send messages over WebSocket
void wsPrint(const String& message) {
String mutableMessage = message; // Create a mutable copy
webSocket.broadcastTXT(mutableMessage);
}
#define RX_PIN 17 // DI
#define TX_PIN 18 // RO
#define DE_PIN 33 // Driver Enable
#define RE_PIN 3 // Receiver Enable
// 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); Feetech servos = Feetech(Serial1, DE_PIN, RE_PIN, TX_PIN, RX_PIN);
// SCS are BIG endian (high byte first) uint16_t flipBytes(uint16_t value) {
// SMS are little endian (low byte first) 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 };
#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() { void setup() {
Serial.begin(115200); Serial.begin(115200);
for (int i = 0; i < 10; i++) {
Serial.println(i);
delay(500);
}
pos2[3] = flipBytes(pos2[3]);
// WiFi.begin(ssid, password);
// while (WiFi.status() != WL_CONNECTED) {
// delay(1000);
// Serial.println("Connecting to WiFi...");
// }
// Serial.println("Connected to WiFi");
// Serial.println("\nConnected!");
// Serial.print("IP address: ");
// Serial.println(WiFi.localIP());
// if (!SPIFFS.begin(false)) {
// Serial.println("SPIFFS mount failed");
// return;
// } else {
// Serial.println("SPIFFS mount initialized");
// }
servos.begin(); servos.begin();
//SetID(34, 15);
// // while (true){ // Serve the file upload form
// // digitalWrite(TX_PIN, !digitalRead(TX_PIN)); // server.on("/", HTTP_GET, [](AsyncWebServerRequest* request) {
// // Serial.println(digitalRead(TX_PIN)); // String html = "<form action='/upload' method='POST' enctype='multipart/form-data'>";
// // delay(2000); // html += "<input type='file' name='file'><br>";
// // } // html += "<input type='submit' value='Upload'>";
// html += "</form>";
// request->send(200, "text/html", html);
// });
// Serial1.begin(1000000, SERIAL_8N1, RX_PIN, TX_PIN); // // Handle file uploads
// pinMode(DE_PIN, OUTPUT); // server.on(
// pinMode(RE_PIN, OUTPUT); // "/upload", HTTP_POST, [](AsyncWebServerRequest* request) {
// SetModeReceive(); // request->send(200, "text/plain", "File uploaded successfully!");
// delay(1000); // },
// handleFileUpload);
// server.begin();
// writeFile("/hello.txt", "Hello ");
// appendFile("/hello.txt", "World!\r\n");
// readFile("/hello.txt");
} }
// Send move command to servo id:0-255, position:0-4095 void readFile(const char* path) {
void sendWritePos(uint8_t id, uint16_t position) { Serial.printf("Reading file: %s\r\n", path);
uint8_t packet[9];
packet[0] = 0xFF; // Header File file = SPIFFS.open(path);
packet[1] = 0xFF; // Header if (!file || file.isDirectory()) {
packet[2] = id; // Servo ID Serial.println("- failed to open file for reading");
packet[3] = 5; // Length = instruction + address + 2 bytes + checksum return;
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 Serial.println("- read from file:");
uint8_t checksum = 0; while (file.available()) {
for (uint8_t i = 2; i < index; i++) { Serial.write(file.read());
checksum += packet[i];
} }
checksum = ~checksum; file.close();
// Send packet
SetModeTransmit();
Serial1.write(packet, index);
Serial1.write(checksum);
Serial1.flush();
SetModeReceive();
} }
// Send a ping command to a specific servo void appendFile(const char* path, const char* message) {
void sendPing(uint8_t id) { Serial.printf("Appending to file: %s\r\n", path);
uint8_t packet[6];
packet[0] = 0xFF; // Header File file = SPIFFS.open(path, FILE_APPEND);
packet[1] = 0xFF; // Header if (!file) {
packet[2] = id; // Servo ID Serial.println("- failed to open file for appending");
packet[3] = 0x02; // Length = instruction + address + checksum return;
packet[4] = PING; // Instruction: PING }
if (file.print(message)) {
Serial.println("- message appended");
} else {
Serial.println("- append failed");
}
file.close();
}
// Calculate checksum void writeFile(const char* path, const char* message) {
uint8_t sum = 0; Serial.printf("Writing file: %s\r\n", path);
for (int i = 2; i < 5; i++) sum += packet[i];
packet[5] = ~sum; // Checksum
// Send packet File file = SPIFFS.open(path, FILE_WRITE);
SetModeTransmit(); if (!file) {
Serial1.write(packet, sizeof(packet)); Serial.println("- failed to open file for writing");
Serial1.flush(); return;
SetModeReceive(); }
if (file.print(message)) {
Serial.println("- file written");
} else {
Serial.println("- write failed");
}
file.close();
}
if (PRINT_SENT_PACKETS) { void listFiles() {
Serial.print("Sent ping packet: "); Serial.println("Files in SPIFFS:");
for (int i = 0; i < 6; i++) { File root = SPIFFS.open("/");
Serial.print("0x"); if (!root || !root.isDirectory()) {
if (packet[i] < 0x10) Serial.print("0"); // Leading zero for single-digit hex Serial.println("Failed to open root directory");
Serial.print(packet[i], HEX); return;
Serial.print(" "); }
}
Serial.println(); File file = root.openNextFile();
bool found = false;
while (file) {
Serial.print("File: ");
Serial.print(file.name());
Serial.print(" | Size: ");
Serial.println(file.size());
file = root.openNextFile();
found = true;
}
if (!found) {
Serial.println("No files found in SPIFFS");
} }
} }
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 printFile(const String& filename) {
File file = SPIFFS.open(filename, "r");
if (!file) {
Serial.println("Failed to open file for reading");
return;
}
Serial.print("Content of ");
Serial.println(filename);
while (file.available()) {
Serial.write(file.read());
}
file.close();
Serial.println(); // New line after file content
}
void handleFileUpload(AsyncWebServerRequest* request, String filename, size_t index, uint8_t* data, size_t len, bool final) {
filename = "/" + filename.substring(filename.lastIndexOf('/') + 1);
if (index == 0) {
Serial.printf("UploadStart: %s\n", filename.c_str());
}
size_t previewLen = len > 256 ? 256 : len; // limit preview
Serial.print("Chunk preview: ");
for (size_t i = 0; i < previewLen; i++) {
Serial.print((char)data[i]);
}
Serial.println();
if (final) {
Serial.printf("UploadEnd: %s (%u)\n", filename.c_str(), index + len);
} }
} }
void SetModeTransmit() { void SetID(uint8_t oldID, uint8_t newID) {
digitalWrite(DE_PIN, HIGH); Serial.println("Setting Lock to 0");
digitalWrite(RE_PIN, HIGH); Serial.println(servos.setLock(oldID, 0));
delay(10); 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 SetModeReceive() { void loop() {
digitalWrite(DE_PIN, LOW); // put your main code here, to run repeatedly:
digitalWrite(RE_PIN, LOW); //PingAll();
delay(10);
servos.syncWritePos(ids, pos1, 4);
delay(2000);
servos.syncWritePos(ids, pos2, 4);
delay(2000);
} }
void PingAll() { void PingAll() {
Serial.println("PINGING ALL 0-255"); std::vector<uint8_t> successfulAddresses;
for (int i = 180; i < 255; i++) { servos.pingAll(successfulAddresses);
sendPing(i); // Now successfulAddresses contains all successful pings
Serial.println("Successful Addresses:");
WaitOnReply(100); for (uint8_t address : successfulAddresses) {
Serial.print(address);
Serial.print(" ");
} }
Serial.println("PINGING COMPLETE"); Serial.println();
}
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);
} }