added serial passthrough define option at astart of HansonServo.ino for using feetech app

HansonServo.ino

@@ -4,21 +4,26 @@
  * Unified robot controller with:
  * - Feetech servo control (SCS/STS)
  * - Animation playback via node graphs
- * - BNO055 IMU
+ * - ADXL345 IMU
  * - RD-03D mmWave radar
  * - CRC16 tagged packet protocol
  *
  * Protocol: 0xA5 0x5A [TAG 4B][LEN 2B][SEQ 2B][PAYLOAD][CRC16 2B]
  */
 
-#include <FFat.h>
+// Configuration defines
-#include "protocol.h"
+#define ENABLE_SERIAL_PASSTHROUGH                                              \
+  false // Set true to use Feetech app comms straight to servos
+
+#include "RobotConfig.h"
+#include "animation.h"
 #include "commands.h"
 #include "motorcontrol.h"
-#include "animation.h"
 #include "nodegraph.h"
-#include "RobotConfig.h"
+#include "protocol.h"
 #include "sensors.h"
+#include <FFat.h>
+
 
 // ============================================================================
 // Global State
@@ -56,6 +61,24 @@ void handleProtocol() {
   }
 }
 
+// ============================================================================
+// Serial Passthrough (USB ↔ Servo Serial)
+// ============================================================================
+
+#if ENABLE_SERIAL_PASSTHROUGH
+void handleSerialPassthrough() {
+  // Forward USB Serial → Servo Serial1
+  while (Serial.available()) {
+    Serial1.write(Serial.read());
+  }
+
+  // Forward Servo Serial1 → USB Serial
+  while (Serial1.available()) {
+    Serial.write(Serial1.read());
+  }
+}
+#endif
+
 // ============================================================================
 // Animation Playback
 // ============================================================================
@@ -80,7 +103,8 @@ void runNodeAnimation() {
   config.enableAllMotors();
 
   uint32_t now = millis();
-  if (now - lastTickTime < FRAME_INTERVAL_MS) return;
+  if (now - lastTickTime < FRAME_INTERVAL_MS)
+    return;
   lastTickTime = now;
 
   // Tick the node graph
@@ -109,14 +133,8 @@ void runNodeAnimation() {
 
   // Send all positions in one sync write - motors move together!
   if (!motorIDs.empty()) {
-    servoManager.syncWritePositions(
+    servoManager.syncWritePositions(motorIDs.data(), positions.data(),
-      motorIDs.data(),
+                                    speeds.data(), motorIDs.size(), config, 0);
-      positions.data(),
-      speeds.data(),
-      motorIDs.size(),
-      config,
-      0
-    );
   }
 
   // Emit per-frame event: [frameLo, frameHi, playMode, status=0]
@@ -174,7 +192,8 @@ void runNodeAnimation() {
 void updateMotorPositions() {
   static unsigned long lastUpdate = 0;
 
-  if (millis() - lastUpdate < MOTOR_UPDATE_INTERVAL_MS) return;
+  if (millis() - lastUpdate < MOTOR_UPDATE_INTERVAL_MS)
+    return;
   lastUpdate = millis();
 
   for (const Motor &motor : config.motors) {
@@ -197,7 +216,8 @@ void handleMotorStreaming() {
 void sendHeartbeat() {
   static unsigned long lastHeartbeat = 0;
 
-  if (millis() - lastHeartbeat < HEARTBEAT_INTERVAL_MS) return;
+  if (millis() - lastHeartbeat < HEARTBEAT_INTERVAL_MS)
+    return;
   lastHeartbeat = millis();
 
   // Pack state: uptime(4) + flags(2)
@@ -206,11 +226,16 @@ void sendHeartbeat() {
   uint16_t flags = 0;
 
   // Build flags
-  if (adxl.isReady()) flags |= 0x01;
+  if (adxl.isReady())
-  if (animState.current && animState.current->isActive()) flags |= 0x02;
+    flags |= 0x01;
-  if (motorStream.active) flags |= 0x04;
+  if (animState.current && animState.current->isActive())
-  if (sensors.isADXLStreamEnabled()) flags |= 0x08;
+    flags |= 0x02;
-  if (sensors.isRadarStreamEnabled()) flags |= 0x10;
+  if (motorStream.active)
+    flags |= 0x04;
+  if (sensors.isADXLStreamEnabled())
+    flags |= 0x08;
+  if (sensors.isRadarStreamEnabled())
+    flags |= 0x10;
 
   payload[0] = uptime & 0xFF;
   payload[1] = (uptime >> 8) & 0xFF;
@@ -265,6 +290,12 @@ void setup() {
 // ============================================================================
 
 void loop() {
+// Serial passthrough (when enabled)
+#if ENABLE_SERIAL_PASSTHROUGH
+  handleSerialPassthrough();
+  return;
+#endif
+
   // Protocol handling
   handleProtocol();