Refactor face tracking control pipeline for smoother arbitration

.gitignore

@@ -47,6 +47,7 @@ pid_log.csv
 *.mp4
 *.avi
 python_host/data/
+python_host/models/vit-emotion/model.safetensors
 
 # Arduino
 *.hex

esp32_firmware/esp32_kait/kait_v2.ino

@@ -17,6 +17,12 @@ const char* MDNS_NAME = "F7OWER_kait";
 // --- OSC 端口 ---
 const int OSC_PORT = 8888;
 
+const int WIFI_BOOT_CONNECT_ATTEMPTS = 24;
+const int WIFI_AUTO_RETRY_ATTEMPTS = 10;
+const int WIFI_MANUAL_RETRY_DEFAULT = 6;
+const int WIFI_RETRY_DELAY_MS = 500;
+const unsigned long WIFI_RETRY_INTERVAL_MS = 6000;
+
 // --- 引脚定义 ---
 const int MOTOR_PWM_PIN = 22;   // PWM 速度控制
 const int MOTOR_DIR_PIN = 23;   // 方向控制
@@ -33,6 +39,9 @@ const int MOTOR_KICK_START_DELAY = 30;   // 启动冲击延时 (ms)
 // 运行时变量
 // ============================================================
 WiFiUDP udp;
+unsigned long lastWifiRetryMs = 0;
+int wifiManualRetryAttempts = WIFI_MANUAL_RETRY_DEFAULT;
+bool mdnsStarted = false;
 
 // Motor state / 电机状态
 struct MotorState {
@@ -69,38 +78,90 @@ void handleSerialCommand();
 // ────────────────────────────────────────────────────────────
 
 // ============================================================
-// WiFi 初始化（Station 模式只）
+// WiFi / mDNS
 // ============================================================
-void setupWiFi() {
+bool connectWifiWithAttempts(int attempts, bool verbose) {
+  attempts = constrain(attempts, 1, 120);
   WiFi.mode(WIFI_STA);
+  WiFi.setSleep(false);
+  WiFi.setAutoReconnect(true);
+  WiFi.persistent(false);
   WiFi.begin(STA_SSID, STA_PASSWORD);
 
-  Serial.print("🔗 连接WiFi中");
-  int retry = 0;
-  while (WiFi.status() != WL_CONNECTED && retry < 20) {
-    delay(500);
-    Serial.print(".");
-    retry++;
+  if (verbose) Serial.print("[Net] Connecting");
+  for (int i = 0; i < attempts; i++) {
+    if (WiFi.status() == WL_CONNECTED) {
+      if (verbose) {
+        Serial.print("\n[Net] Connected, IP: ");
+        Serial.println(WiFi.localIP());
+      }
+      return true;
+    }
+    delay(WIFI_RETRY_DELAY_MS);
+    if (verbose) Serial.print(".");
   }
 
-  if (WiFi.status() == WL_CONNECTED) {
-    Serial.print("\n✅ WiFi已连接，IP: ");
-    Serial.println(WiFi.localIP());
-  } else {
-    Serial.println("\n❌ WiFi连接失败，请检查 STA_SSID / STA_PASSWORD");
+  if (verbose) {
+    Serial.println("\n[Net] STA connect failed");
   }
+  return WiFi.status() == WL_CONNECTED;
 }
 
-// ============================================================
-// mDNS 初始化
-// ============================================================
-void setupmDNS() {
-  if (MDNS.begin(MDNS_NAME)) {
-    Serial.printf("✅ mDNS 已启动: http://%s.local\n", MDNS_NAME);
-    MDNS.addService("osc", "udp", OSC_PORT);
+void setupNetwork() {
+  if (!connectWifiWithAttempts(WIFI_BOOT_CONNECT_ATTEMPTS, true)) {
+    Serial.println("[Net] Boot without WiFi, auto-retry enabled");
+  }
+}
+
+void ensureWifiConnected() {
+  if (WiFi.status() == WL_CONNECTED) return;
+  unsigned long now = millis();
+  if (now - lastWifiRetryMs < WIFI_RETRY_INTERVAL_MS) return;
+  lastWifiRetryMs = now;
+  Serial.println("[Net] WiFi disconnected, retrying...");
+  connectWifiWithAttempts(WIFI_AUTO_RETRY_ATTEMPTS, false);
+}
+
+void setupMDNS() {
+  if (mdnsStarted) return;
+  if (WiFi.status() != WL_CONNECTED) return;
+  if (!MDNS.begin(MDNS_NAME)) {
+    Serial.println("[Net] mDNS failed");
+    return;
+  }
+  MDNS.addService("osc", "udp", OSC_PORT);
+  MDNS.addService("datt_flower", "tcp", OSC_PORT);
+  MDNS.addServiceTxt("datt_flower", "tcp", "node_type", "kait");
+  MDNS.addServiceTxt("datt_flower", "tcp", "node_id", MDNS_NAME);
+  mdnsStarted = true;
+  Serial.printf("[Net] mDNS ready: %s.local\n", MDNS_NAME);
+}
+
+void ensureMDNS() {
+  if (!mdnsStarted && WiFi.status() == WL_CONNECTED) {
+    setupMDNS();
+  }
+}
+
+void printWifiStatus() {
+  wl_status_t st = WiFi.status();
+  Serial.println("\n=== WiFi Status ===");
+  Serial.printf("SSID: %s\n", STA_SSID);
+  Serial.printf("Status: %d\n", (int)st);
+  if (st == WL_CONNECTED) {
+    Serial.printf("IP: %d.%d.%d.%d\n", WiFi.localIP()[0], WiFi.localIP()[1], WiFi.localIP()[2], WiFi.localIP()[3]);
+    Serial.printf("RSSI: %d dBm\n", WiFi.RSSI());
   } else {
-    Serial.println("❌ mDNS 启动失败");
+    Serial.println("IP: (not connected)");
   }
+  Serial.println("===================\n");
+}
+
+void manualWifiRetry(int attempts) {
+  wifiManualRetryAttempts = constrain(attempts, 1, 120);
+  Serial.printf("[Net] Manual retry, attempts=%d\n", wifiManualRetryAttempts);
+  bool ok = connectWifiWithAttempts(wifiManualRetryAttempts, true);
+  if (ok) ensureMDNS();
 }
 
 // ============================================================
@@ -331,6 +392,8 @@ void handleSerialCommand() {
     Serial.println("\n=== 串口命令帮助 ===");
     Serial.println("motor <speed>  - 设置电机速度 (-255 ~ 255)");
     Serial.println("motion <mode>  - 执行运动模式 (1-6)");
+    Serial.println("wifi status    - 查看 WiFi 状态");
+    Serial.println("wifi retry <n> - 手动重试 WiFi 连接");
     Serial.println("stop           - 停止电机");
     Serial.println("info           - 显示设备信息");
     Serial.println("help           - 显示此帮助");
@@ -349,6 +412,12 @@ void handleSerialCommand() {
       motorState.isRunning ? "运行中" : "停止",
       motorState.currentSpeed);
     Serial.println("====================\n");
+  } else if (line.equals("wifi status")) {
+    printWifiStatus();
+  } else if (line.startsWith("wifi retry")) {
+    int attempts = wifiManualRetryAttempts;
+    sscanf(line.c_str(), "wifi retry %d", &attempts);
+    manualWifiRetry(attempts);
   }
 }
 
@@ -368,8 +437,8 @@ void setup() {
   Serial.println("\n========== F7OWER Kait Node v2 ==========");
   Serial.println("设置 WiFi 连接...");
 
-  setupWiFi();
-  setupmDNS();
+  setupNetwork();
+  setupMDNS();
 
   udp.begin(OSC_PORT);
   Serial.printf("✅ OSC 监听端口: %d\n", OSC_PORT);
@@ -400,6 +469,9 @@ void loop() {
   // 串口命令处理
   handleSerialCommand();
 
+  ensureWifiConnected();
+  ensureMDNS();
+
   // 自动序列（如果激活）
   runAutoSequence();
 }

esp32_firmware/esp32_sue/esp32_sue.ino

@@ -0,0 +1,126 @@
+#include <Arduino_GFX_Library.h>
+#include <ESP32Servo.h>
+
+// DISPLAY
+#define TFT_MOSI 23
+#define TFT_SCLK 18
+#define TFT_CS   19
+#define TFT_DC   21
+#define TFT_RST  22
+
+Arduino_DataBus *bus  = new Arduino_ESP32SPI(TFT_DC, TFT_CS, TFT_SCLK, TFT_MOSI, -1);
+Arduino_GFX    *panel = new Arduino_GC9A01(bus, TFT_RST, 0);
+
+// Local canvas covering only the pupil + iris region
+// Iris radius = 30, pupil radius = 13, pupil travels x = 110~130
+// Bounding box with margin: x=90~150 (w=60), y=84~156 (h=72)
+#define CANVAS_X  90
+#define CANVAS_Y  84
+#define CANVAS_W  60
+#define CANVAS_H  72
+
+Arduino_GFX *sprite = new Arduino_Canvas(CANVAS_W, CANVAS_H, panel, CANVAS_X, CANVAS_Y);
+
+// Local-space center (offset from canvas origin)
+#define LCX  (120 - CANVAS_X)   // 30
+#define LCY  (120 - CANVAS_Y)   // 36
+
+// SERVO
+Servo servo1;
+#define SERVO1_PIN 4
+
+// COLORS
+#define BLACK      0x0000
+#define WHITE      0xFFFF
+#define SCLERA     0xEF7D
+#define IRIS_DARK  0x0015
+#define IRIS_MID   0x027F
+#define IRIS_LIGHT 0x3DFF
+#define SHADOW     0x4208
+
+#define CX 120
+#define CY 120
+
+
+// Draw full static eye directly to panel — called ONCE in setup()
+void drawStaticEye()
+{
+  panel->fillScreen(BLACK);
+  // eyeball
+  panel->fillCircle(CX, CY, 65, SCLERA);
+  // iris
+  panel->fillCircle(CX, CY, 30, IRIS_DARK);
+  panel->fillCircle(CX, CY, 24, IRIS_MID);
+  panel->fillCircle(CX, CY, 16, IRIS_LIGHT);
+
+  panel->fillRoundRect(35, 40, 170, 20, 10, SHADOW);
+}
+
+
+// Redraw pupil region into sprite buffer, flush patch to screen
+void drawPupil(int pupilX)
+{
+  int lx = pupilX - CANVAS_X;   // pupil x in local canvas space
+
+  // clear sprite and redraw iris layers that fall inside this region
+  sprite->fillScreen(BLACK);
+  sprite->fillCircle(LCX, LCY, 65, SCLERA);
+  sprite->fillCircle(LCX, LCY, 30, IRIS_DARK);
+  sprite->fillCircle(LCX, LCY, 24, IRIS_MID);
+  sprite->fillCircle(LCX, LCY, 16, IRIS_LIGHT);
+
+  // pupil + highlight in local coords
+  sprite->fillCircle(lx,     LCY,     13, BLACK);
+  sprite->fillCircle(lx - 4, LCY - 6,  4, WHITE);
+
+  // push sprite patch to screen — only 60×72 = 4320 pixels over SPI
+  ((Arduino_Canvas *)sprite)->flush();
+}
+
+
+void setup()
+{
+  Serial.begin(115200);
+  Serial.print("MaxAllocHeap: "); Serial.println(ESP.getMaxAllocHeap());
+
+  panel->begin();
+  panel->setRotation(2);
+
+  Serial.println("panel ok");
+
+  sprite->begin();
+
+  Serial.println("sprite ok");
+
+  servo1.setPeriodHertz(50);
+  servo1.attach(SERVO1_PIN, 500, 2400);
+
+  drawStaticEye();   // full background drawn once to panel GRAM
+  drawPupil(CX);     // initial pupil
+}
+
+
+void loop()
+{
+  // sweep 0 → 180
+  for (int pos = 0; pos <= 120; pos++)
+  {
+    servo1.write(pos);
+    int pupilX = map(pos, 0, 180, 110, 130);
+    drawPupil(pupilX);
+    delay(2);
+  }
+
+  delay(500);
+
+  // sweep 180 → 0
+  for (int pos = 100; pos >= 0; pos--)
+  {
+    servo1.write(pos);
+    int pupilX = map(pos, 0, 180, 110, 130);
+    drawPupil(pupilX);
+    delay(2);
+  }
+
+  delay(500);
+}