291 lines
7.0 KiB
C++
291 lines
7.0 KiB
C++
#include "feetech.h"
|
|
#include <WiFi.h>
|
|
#include <AsyncTCP.h>
|
|
#include <ESPAsyncWebServer.h>
|
|
#include <WebSocketsServer.h>
|
|
|
|
|
|
#include <SPIFFS.h>
|
|
|
|
// 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
|
|
|
|
|
|
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]);
|
|
// 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);
|
|
|
|
// Serve the file upload form
|
|
// server.on("/", HTTP_GET, [](AsyncWebServerRequest* request) {
|
|
// String html = "<form action='/upload' method='POST' enctype='multipart/form-data'>";
|
|
// html += "<input type='file' name='file'><br>";
|
|
// html += "<input type='submit' value='Upload'>";
|
|
// html += "</form>";
|
|
// request->send(200, "text/html", html);
|
|
// });
|
|
|
|
// // 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");
|
|
}
|
|
|
|
void readFile(const char* path) {
|
|
Serial.printf("Reading file: %s\r\n", path);
|
|
|
|
File file = SPIFFS.open(path);
|
|
if (!file || file.isDirectory()) {
|
|
Serial.println("- failed to open file for reading");
|
|
return;
|
|
}
|
|
|
|
Serial.println("- read from file:");
|
|
while (file.available()) {
|
|
Serial.write(file.read());
|
|
}
|
|
file.close();
|
|
}
|
|
|
|
void appendFile(const char* path, const char* message) {
|
|
Serial.printf("Appending to file: %s\r\n", path);
|
|
|
|
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();
|
|
}
|
|
|
|
void writeFile(const char* path, const char* message) {
|
|
Serial.printf("Writing file: %s\r\n", path);
|
|
|
|
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();
|
|
}
|
|
|
|
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 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 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();
|
|
|
|
|
|
servos.syncWritePos(ids, pos1, 4);
|
|
delay(2000);
|
|
servos.syncWritePos(ids, pos2, 4);
|
|
delay(2000);
|
|
}
|
|
|
|
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();
|
|
}
|