From d7ff7b31558773e63fb14039d2a1ba521886b491 Mon Sep 17 00:00:00 2001
From: Jake <realrobotshk@gmail.com>
Date: Mon, 29 Sep 2025 23:05:13 +0800
Subject: [PATCH] sync option implemented, motors move with dials and timeline
 shifts

---
 HansonServo.ino | 76 +++++++++++++++++++++++++++++++++++++++++++------
 1 file changed, 67 insertions(+), 9 deletions(-)

diff --git a/HansonServo.ino b/HansonServo.ino
index ea435a2..944b3e7 100644
--- a/HansonServo.ino
+++ b/HansonServo.ino
@@ -17,6 +17,7 @@ uint8_t payload[MAX_PAYLOAD_SIZE];  // Global or static
 #define CMD_DELETE_FILE 0x04
 #define CMD_SAVE_FILE 0x05
 #define CMD_MESSAGE 0x06
+#define CMD_SET_POSITION 0x07
 
 
 // ESP32 S2 PINOUT
@@ -166,6 +167,10 @@ void handleCommand(uint8_t command, const uint8_t* payload, uint16_t length) {
       handleSaveFile(payload, length);
       break;
 
+    case CMD_SET_POSITION:
+      handleSetPosition(payload, length);
+      break;
+
     default:
       Serial.print("Unknown command: ");
       Serial.println(command, HEX);
@@ -173,7 +178,7 @@ void handleCommand(uint8_t command, const uint8_t* payload, uint16_t length) {
   }
 }
 
-void sendMessage(const String& payload) {
+void sendMessage(const String& payload, uint8_t command = CMD_MESSAGE) {
   uint16_t length = payload.length();
 
   uint8_t checksum = CMD_MESSAGE ^ (length >> 8) ^ (length & 0xFF);
@@ -183,14 +188,13 @@ void sendMessage(const String& payload) {
 
   Serial.write(HEADER1);
   Serial.write(HEADER2);
-  Serial.write(CMD_MESSAGE);
+  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 handleIdRequest() {
   String payload = String(DEVICE_NAME) + "|" + FIRMWARE_VERSION;
   uint16_t length = payload.length();
@@ -309,6 +313,32 @@ void handleLoadFile(const uint8_t* payload, uint16_t length) {
 }
 
 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 handleSaveFile(const uint8_t* payload, uint16_t length) {
@@ -372,9 +402,7 @@ bool parseAnimationPayload(const uint8_t* payload, uint16_t length, Animation& a
     return false;
   }
 
-  char filename[filenameLength+1];
-
-  //strcpy(filename, "/test1.anim");
+  char filename[filenameLength + 1];
 
 
   memcpy(filename, payload + 2, filenameLength);
@@ -427,18 +455,47 @@ bool parseAnimationPayload(const uint8_t* payload, uint16_t length, Animation& a
 }
 
 
+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.syncWritePos(ids, pos1, NUM_CHANNELS);
+  // String msg = "Set positions: ";
+  // for (int i = 0; i < count; i++) {
+  //   msg += "ID " + String(ids[i]) + " → " + String(pos1[i]) + "; ";
+  // }
+  // sendMessage(msg.c_str(), CMD_SET_POSITION);
+}
 
 
 
 
-
-
+bool flip = false;
 unsigned long lastSend = 0;
 void loop() {
   HandleSerialRequests();
   // put your main code here, to run repeatedly:
   //PingAll();
 
+  // for (int i = 0; i < 1023; i++) {
+  //   for (int z = 0; z < 5; z++) {
+  //     pos1[z] = i;
+  //   }
+  //   servos.syncWritePos(ids, pos1, 5);
+  //   delay(50);
+  // }
 
   // playAnimation(sweep);
   // playAnimation(stagger);
@@ -451,7 +508,8 @@ void loop() {
   // servos.syncWritePos(ids, pos2, 5);
   // delay(1000);
 
-  if (millis() - lastSend > 1000) {
+  if (millis() - lastSend > 2000) {
+
     //sendMessageFromESP32(String(millis()));
     //handleIdRequest();
     //PrintFileList();