diff --git a/feetech.cpp b/feetech.cpp index 7471704..51ac339 100644 --- a/feetech.cpp +++ b/feetech.cpp @@ -7,10 +7,54 @@ Feetech::Feetech(HardwareSerial& serial, int DE_PIN, int RE_PIN, int TX_PIN, int void Feetech::begin() { serial.begin(1000000, SERIAL_8N1, RX_PIN, TX_PIN); pinMode(DE_PIN, OUTPUT); - pinMode(RE_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]; @@ -51,9 +95,12 @@ void Feetech::sendPing(uint8_t id) { packet[5] = ~sum; // Checksum // Send packet + //Serial.println("PING"); setModeTransmit(); + //delay(20); serial.write(packet, sizeof(packet)); serial.flush(); + //delay(20); setModeReceive(); } @@ -104,6 +151,11 @@ uint8_t Feetech::getID(uint8_t id) { 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); @@ -165,7 +217,17 @@ uint16_t Feetech::getGoalSpeed(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); } @@ -183,7 +245,7 @@ uint16_t Feetech::getPosition(uint8_t id) { int16_t Feetech::getSpeed(uint8_t id) { sendRequest(id, REQUEST_CURRENT_SPEED, 2); int16_t val = waitOnData2Bytes(10); - if (val < 0){ + if (val < 0) { val -= 32767; 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[4] = READ_DATA; // Instruction packet[5] = instruction; // Write first address - packet[6] = byteCount; // Number of bytes to read + packet[6] = byteCount; // Number of bytes to read // Calculate checksum uint8_t sum = 0; @@ -234,12 +296,62 @@ void Feetech::sendRequest(uint8_t id, byte instruction, uint8_t byteCount) { setModeReceive(); } -void Feetech::pingAll() { - Serial.println("PINGING ALL 0-255"); - for (int i = 0; i < 255; i++) { - sendPing(i); +void Feetech::write1Byte(uint8_t id, byte instruction, uint8_t data) { + uint8_t packet[8]; - 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& 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"); } @@ -249,7 +361,7 @@ void Feetech::waitOnReply(unsigned long timeout) { while (millis() - startTime < timeout) { // Loop until timeout if (serial.available()) { - Serial.println("RECV"); + //Serial.println("RECV"); uint8_t buffer[32]; int count = 0; @@ -258,7 +370,7 @@ void Feetech::waitOnReply(unsigned long timeout) { buffer[count++] = serial.read(); } - Serial.println(count); + //Serial.println(count); // Display on Serial Serial.print("recv: "); @@ -290,7 +402,7 @@ uint8_t Feetech::waitOnData1Byte(unsigned long timeout) { while (millis() - startTime < timeout) { // Loop until timeout if (serial.available()) { - Serial.println("RECV"); + //Serial.println("RECV"); uint8_t buffer[32]; int count = 0; @@ -299,7 +411,7 @@ uint8_t Feetech::waitOnData1Byte(unsigned long timeout) { buffer[count++] = serial.read(); } - Serial.println(count); + //Serial.println(count); // if (count != 8) { // Serial.print("ERROR: Expected 8 byte reply, recieved "); // Serial.println(count); @@ -329,9 +441,12 @@ uint8_t Feetech::waitOnData1Byte(unsigned long timeout) { return val; 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) { @@ -379,8 +494,9 @@ uint16_t Feetech::waitOnData2Bytes(unsigned long timeout) { 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) { diff --git a/feetech.h b/feetech.h index 4b19c2b..e79bb68 100644 --- a/feetech.h +++ b/feetech.h @@ -1,6 +1,7 @@ #ifndef FEETECH_H #define FEETECH_H +#include #include #include @@ -15,6 +16,7 @@ public: 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); @@ -28,6 +30,8 @@ public: 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); @@ -41,7 +45,10 @@ public: 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 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& successfulAddresses); void waitOnReply(unsigned long timeout); uint8_t waitOnData1Byte(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 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 @@ -78,7 +86,7 @@ public: 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 = 0x37; // 1 byte + static const byte REQUEST_LOCK = 0x30; // 1 byte 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_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) static const byte SMS_STS_1M = 0; diff --git a/rs485transmitter.ino b/rs485transmitter.ino index b7a111e..c7deaae 100644 --- a/rs485transmitter.ino +++ b/rs485transmitter.ino @@ -1,470 +1,290 @@ #include "feetech.h" +#include +#include +#include +#include +#include -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 +// Replace with your network credentials +const char* ssid = "Police Surveillance Van"; +const char* password = "ourpassword"; +// 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); -// SCS are BIG endian (high byte first) -// SMS are little endian (low byte first) +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 }; -#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); + 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(); + //SetID(34, 15); - // // while (true){ - // // digitalWrite(TX_PIN, !digitalRead(TX_PIN)); - // // Serial.println(digitalRead(TX_PIN)); - // // delay(2000); - // // } + // Serve the file upload form + // server.on("/", HTTP_GET, [](AsyncWebServerRequest* request) { + // String html = "
"; + // html += "
"; + // html += ""; + // html += "
"; + // request->send(200, "text/html", html); + // }); - // Serial1.begin(1000000, SERIAL_8N1, RX_PIN, TX_PIN); - // pinMode(DE_PIN, OUTPUT); - // pinMode(RE_PIN, OUTPUT); - // SetModeReceive(); - // delay(1000); + // // Handle file uploads + // server.on( + // "/upload", HTTP_POST, [](AsyncWebServerRequest* request) { + // request->send(200, "text/plain", "File uploaded successfully!"); + // }, + // 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 sendWritePos(uint8_t id, uint16_t position) { - uint8_t packet[9]; +void readFile(const char* path) { + Serial.printf("Reading file: %s\r\n", path); - 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 + File file = SPIFFS.open(path); + if (!file || file.isDirectory()) { + Serial.println("- failed to open file for reading"); + return; } - // Checksum - uint8_t checksum = 0; - for (uint8_t i = 2; i < index; i++) { - checksum += packet[i]; + Serial.println("- read from file:"); + while (file.available()) { + Serial.write(file.read()); } - checksum = ~checksum; - - // Send packet - SetModeTransmit(); - Serial1.write(packet, index); - Serial1.write(checksum); - Serial1.flush(); - SetModeReceive(); + file.close(); } -// Send a ping command to a specific servo -void sendPing(uint8_t id) { - uint8_t packet[6]; +void appendFile(const char* path, const char* message) { + Serial.printf("Appending to file: %s\r\n", path); - 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 + File file = SPIFFS.open(path, FILE_APPEND); + if (!file) { + Serial.println("- failed to open file for appending"); + return; + } + if (file.print(message)) { + Serial.println("- message appended"); + } else { + Serial.println("- append failed"); + } + file.close(); +} - // Calculate checksum - uint8_t sum = 0; - for (int i = 2; i < 5; i++) sum += packet[i]; - packet[5] = ~sum; // Checksum +void writeFile(const char* path, const char* message) { + Serial.printf("Writing file: %s\r\n", path); - // Send packet - SetModeTransmit(); - Serial1.write(packet, sizeof(packet)); - Serial1.flush(); - SetModeReceive(); + File file = SPIFFS.open(path, FILE_WRITE); + if (!file) { + Serial.println("- failed to open file for writing"); + return; + } + if (file.print(message)) { + Serial.println("- file written"); + } else { + Serial.println("- write failed"); + } + file.close(); +} - 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 listFiles() { + Serial.println("Files in SPIFFS:"); + File root = SPIFFS.open("/"); + if (!root || !root.isDirectory()) { + Serial.println("Failed to open root directory"); + return; + } + + 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() { - digitalWrite(DE_PIN, HIGH); - digitalWrite(RE_PIN, HIGH); - delay(10); +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 SetModeReceive() { - digitalWrite(DE_PIN, LOW); - digitalWrite(RE_PIN, LOW); - delay(10); +void loop() { + // put your main code here, to run repeatedly: + //PingAll(); + + + servos.syncWritePos(ids, pos1, 4); + delay(2000); + servos.syncWritePos(ids, pos2, 4); + delay(2000); } void PingAll() { - Serial.println("PINGING ALL 0-255"); - for (int i = 180; i < 255; i++) { + std::vector successfulAddresses; + servos.pingAll(successfulAddresses); - sendPing(i); - - WaitOnReply(100); + // Now successfulAddresses contains all successful pings + Serial.println("Successful Addresses:"); + 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); -} \ No newline at end of file