added passthrough for feetech app

HansonServo.ino

@@ -15,6 +15,10 @@ RobotConfig config;
 #define MAX_PAYLOAD_SIZE 6000       // 10 KB
 uint8_t payload[MAX_PAYLOAD_SIZE];  // Global or static
 
+#define MODE_NORMAL 0
+#define MODE_FEETECH 1
+uint8_t mode = MODE_FEETECH;
+
 #define CMD_ID_REQUEST 0x01
 #define CMD_FILE_LIST 0x02
 #define CMD_LOAD_FILE 0x03
@@ -82,25 +86,13 @@ uint16_t getSineWaveValue(unsigned long centiseconds) {
 
 void setup() {
   Serial.begin(1000000);
+  //Serial1.begin(1000000, SERIAL_8N1, CH0_RX_PIN, CH0_TX_PIN);
   Serial.setRxBufferSize(1024);
   for (int i = 0; i < 5; i++) {
     Serial.println(i);
     delay(500);
   }
 
-  // config.deviceName = "Little Elephant";
-  // config.firmwareVersion.major = 0;
-  // config.firmwareVersion.minor = 2;
-
-  // Add a few motors
-  // config.motors.push_back({ "Ankle Left", 10, 2048 });
-  // config.motors.push_back({ "Ankle Right", 11, 2048 });
-  // config.motors.push_back({ "Ankle Center", 12, 2048 });
-  // config.motors.push_back({ "Knee", 13, 2048 });
-  // config.motors.push_back({ "Upper Leg", 14, 2048 });
-
-
-
   pinMode(6, INPUT);
   pinMode(7, INPUT);
 
@@ -109,22 +101,8 @@ void setup() {
   servos[1] = new Feetech(Serial2, DE_PIN, RE_PIN, CH1_TX_PIN, CH1_RX_PIN);  // STS
   servos[1]->setFeetechMode(Feetech::MODE_SCS);
 
-  // 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();
+  // servos[1]->begin();
 
   if (!FFat.begin(true)) {
     Serial.println("FFat mount failed");
@@ -190,6 +168,7 @@ void HandleSerialRequests() {
       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
@@ -222,6 +201,7 @@ void HandleSerialRequests() {
     }
   }
 }
+
 void sendMessage(const String& payload, uint8_t command = CMD_MESSAGE) {
   uint16_t length = payload.length();
 
@@ -389,7 +369,7 @@ void handleDataWrite(const uint8_t* payload, uint16_t length) {
   uint8_t id = payload[1];
   uint8_t writeByte = payload[2];
 
-  uint8_t model = servos[payload[0]]->getMajorModel(id);//  config.getMotorModel(id);
+  uint8_t model = servos[payload[0]]->getMajorModel(id);  //  config.getMotorModel(id);
   sendMessage(String(model));
   servos[payload[0]]->setFeetechMode(model);  // put feetech interface in correct mode
 
@@ -952,10 +932,52 @@ void handleScanChannel(const uint8_t* payload, uint16_t length) {
 }
 
 
+// Target packet: 0xAA, 0x55, 0x01, 0x00, 0x00, 0x01
+const uint8_t targetPacket[6] = {0xAA, 0x55, 0x01, 0x00, 0x00, 0x01};
+uint8_t sniffBuffer[6];
+uint8_t sniffIndex = 0;
+
+void sniffByte(uint8_t b) {
+  // Push into buffer
+  sniffBuffer[sniffIndex++] = b;
+  if (sniffIndex == 6) {
+    // Compare with target
+    bool match = true;
+    for (int i = 0; i < 6; i++) {
+      if (sniffBuffer[i] != targetPacket[i]) {
+        match = false;
+        break;
+      }
+    }
+    if (match) {
+      Serial.println("Detected FEETECH packet 0xAA 0x55 0x01 0x00 0x00 0x01!");
+      mode = MODE_NORMAL;
+      handleIdRequest();
+      return;
+    }
+    // Slide window: keep last 5 bytes
+    for (int i = 1; i < 6; i++) sniffBuffer[i-1] = sniffBuffer[i];
+    sniffIndex = 5;
+  }
+}
 
 bool flip = false;
 unsigned long lastSend = 0;
 void loop() {
+  if (mode == MODE_FEETECH) {
+    if (Serial.available()) {
+      char c = Serial.read();
+      Serial1.write(c);
+      sniffByte((uint8_t)c);   // sniff traffic PC → UART
+    }
+
+    // Forward data from Serial1 (UART device) to Serial (PC)
+    if (Serial1.available()) {
+      char c = Serial1.read();
+      Serial.write(c);
+    }
+    return;
+  }
 
 
   runNodeAnimation();
@@ -1110,12 +1132,12 @@ void SendMotorPositions() {
     payload.push_back(position & 0xFF);  // Low byte
     payload.push_back(position >> 8);    // High byte
     //Serial.print(position);
-    
+
     //Serial.print("\t");
     // Serial.print(position & 0xFF);
     // Serial.print(",\t");
   }
-   //Serial.println();
+  //Serial.println();
 
   sendMessage(payload.data(), payload.size(), POSITION_STREAM);
 }