Add AP+STA WiFi manager and stabilize balancing

- Replace wifi.cpp/wifi.h with wifi_manager.cpp/wifi_manager.h: AP and STA
  run together, external WiFi credentials are stored in NVS and editable at
  /wifi, with optional static IP, 15s auto-reconnect, disconnect reason
  reporting, and build/version diagnostics
- Fix MPU6050 I2C port to I2C0 to match the board wiring
- Initialize distance_zeropoint at startup and on Robot Go! rising edge
- Clamp angle_zeropoint to +/-10 degrees to prevent balance-point drift
- Add DEBUG_DISABLE_YAW switch and periodic balance debug output
- Update WiFi setup instructions in README.md and README_CN.md

3.Software/wl_pro_robot/basic_web.h

@@ -280,6 +280,9 @@ const char basic_web[] PROGMEM = R"=====(
 <!-- onload 事件在页面载入完成后立即触发 -->
 <body onload="javascript:socket_init()">
     <h2>WL-PRO WiFi遥控模式</h2>
+    <div style="text-align:center;margin:-4px 0 10px;">
+        <a href="/wifi" style="color:#6495ed;font-size:15px;">WiFi 设置 / WiFi Settings</a>
+    </div>
     <div class="view2">
         <input type="checkbox" id="stable" class="switch" onclick="is_stable()" style="vertical-align:middle">Robot Go!</input>
     </div>
@@ -334,7 +337,7 @@ const char basic_web[] PROGMEM = R"=====(
         function socket_init() {
             // 初始化websocket客户端
             //socket = new WebSocket('ws://' + '192.168.4.1' + ':81/'); // AP模式
-            socket = new WebSocket('ws://' + window.location.hostname + ':81/'); // sta模式
+            socket = new WebSocket('ws://' + window.location.hostname + ':81/'); // AP/STA通用
         }
 
         function setroll() {

3.Software/wl_pro_robot/wifi.cpp

@@ -1,66 +0,0 @@
-// -----------------------------------------------------------------------------
-// Copyright (c) 2024 Mu Shibo
-// 
-// Permission is hereby granted, free of charge, to any person obtaining a copy
-// of this software and associated documentation files (the "Software"), to deal
-// in the Software without restriction, including without limitation the rights
-// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-// copies of the Software, and to permit persons to whom the Software is
-// furnished to do so, subject to the following conditions:
-// 
-// The above copyright notice and this permission notice shall be included in
-// all copies or substantial portions of the Software.
-// 
-// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-// THE SOFTWARE.
-// -----------------------------------------------------------------------------
-
-#include "wifi.h"
-
-// 配置AP（热点）模式相关参数
-const char *AP_SSID = "WLROBOT"; 
-const char *AP_PSW = "12345678";    
-
-IPAddress AP_IP(192, 168, 1, 11); 
-IPAddress AP_GATEWAY(192, 168, 1, 11); 
-IPAddress AP_SUBNET(255, 255, 255, 0); 
-
-
-// 配置STA模式相关参数
-char *sta_ssid = "MUJITECH";
-char *sta_password = "mujitech";
-
-
-
-void WiFi_SetAP(void)
-{
-	WiFi.mode(WIFI_AP); 
-	WiFi.softAPConfig(AP_IP, AP_GATEWAY, AP_SUBNET); 
-	WiFi.softAP(AP_SSID, AP_PSW); 
-	// Serial.println();
-	// Serial.print("AP IP address = ");
-	// Serial.println(WiFi.softAPIP());
-}
-
-
-// ESP-01S AP模式接入WiFi网络
-void set_sta_wifi()
-{
-
-  WiFi.begin(sta_ssid, sta_password);
-  // 等待连接
-  while(WiFi.status()!=WL_CONNECTED)
-  {
-    //Serial.print(".");
-    delay(500);
-  }
-  // 打印ESP-01S的IP地址
-  Serial.println("");
-  Serial.print("IP Address: ");
-  Serial.println(WiFi.localIP());
-}

3.Software/wl_pro_robot/wifi_manager.cpp

@@ -0,0 +1,638 @@
+// -----------------------------------------------------------------------------
+// Copyright (c) 2024 Mu Shibo
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+// THE SOFTWARE.
+// -----------------------------------------------------------------------------
+
+#include "wifi_manager.h"
+
+#include <Arduino.h>
+#include <Esp.h>
+#include <Preferences.h>
+
+// ---------------------------------------------------------------------------
+// 热点（AP）参数：作为外部 WiFi 连不上时的备用入口
+// ---------------------------------------------------------------------------
+const char *AP_SSID = "WLROBOT";
+const char *AP_PSW = "12345678";
+
+// 注意：192.168.4.x 是 ESP32 的常用热点网段，避免和多数家用路由器的
+// 192.168.1.x 冲突。如果你的路由器本身就是 192.168.4.x，请改这里。
+IPAddress AP_IP(192, 168, 4, 1);
+IPAddress AP_GATEWAY(192, 168, 4, 1);
+IPAddress AP_SUBNET(255, 255, 255, 0);
+
+// 首次烧录、Flash 里还没有保存过配置时，沿用原代码里的外部 WiFi。
+// 如果不希望自动连它，在 /wifi 页面清空 SSID 保存即可。
+static const char *DEFAULT_STA_SSID = "MUJITECH";
+static const char *DEFAULT_STA_PASS = "mujitech";
+
+// ---------------------------------------------------------------------------
+// 配置存储
+// ---------------------------------------------------------------------------
+struct WiFiConfig {
+  String ssid;
+  String password;
+  bool use_static = false;
+  String ip;
+  String gateway;
+  String subnet;
+  String dns;
+};
+
+static const char *NVS_NAMESPACE = "wifi_cfg";
+
+// 编译时间：打开 http://192.168.4.1/wifi/version 可确认板子里跑的是不是这份代码
+static const char *WIFI_CFG_BUILD = __DATE__ " " __TIME__;
+
+static WiFiConfig wifi_cfg;
+static bool wifi_cfg_active = false;  // SSID 非空，表示需要连接外部 WiFi
+static WebServer *wifi_server = nullptr;
+static uint32_t restart_at_ms = 0;
+static uint32_t last_retry_ms = 0;
+static bool sta_was_connected = false;
+static int last_disconnect_reason = -1;  // 最近一次 STA 断开/连接失败的 reason
+
+static const char *wifiReasonText(uint8_t reason) {
+  switch (reason) {
+    case 2: return "AUTH_EXPIRE";
+    case 3: return "AUTH_LEAVE";
+    case 4: return "ASSOC_EXPIRE";
+    case 5: return "ASSOC_TOOMANY";
+    case 6: return "NOT_AUTHED";
+    case 7: return "NOT_ASSOCED";
+    case 8: return "ASSOC_LEAVE";
+    case 9: return "ASSOC_NOT_AUTHED";
+    case 15: return "4WAY_HANDSHAKE_TIMEOUT（密码错误/加密不兼容）";
+    case 16: return "GROUP_KEY_UPDATE_TIMEOUT";
+    case 17: return "IE_IN_4WAY_DIFFERS";
+    case 18: return "GROUP_CIPHER_INVALID";
+    case 19: return "PAIRWISE_CIPHER_INVALID";
+    case 20: return "AKMP_INVALID";
+    case 23: return "802_1X_AUTH_FAILED（企业级加密）";
+    case 24: return "CIPHER_SUITE_REJECTED（加密方式不支持）";
+    case 200: return "BEACON_TIMEOUT";
+    case 201: return "NO_AP_FOUND（SSID/2.4GHz/信号）";
+    case 202: return "AUTH_FAIL（密码）";
+    case 203: return "ASSOC_FAIL";
+    case 204: return "HANDSHAKE_TIMEOUT（密码）";
+    case 205: return "CONNECTION_FAIL（路由器拒绝/信号）";
+    default: return "UNKNOWN";
+  }
+}
+
+static void onWiFiEvent(arduino_event_id_t event, arduino_event_info_t info) {
+  if (event == ARDUINO_EVENT_WIFI_STA_CONNECTED) {
+    Serial.println("[WiFi] STA associated, waiting for IP...");
+  } else if (event == ARDUINO_EVENT_WIFI_STA_GOT_IP) {
+    last_disconnect_reason = -1;
+    Serial.printf("[WiFi] STA got IP: %s\n", WiFi.localIP().toString().c_str());
+  } else if (event == ARDUINO_EVENT_WIFI_STA_DISCONNECTED) {
+    last_disconnect_reason = info.wifi_sta_disconnected.reason;
+    Serial.printf("[WiFi] STA disconnected/failed, reason=%d (%s)\n",
+                  last_disconnect_reason,
+                  wifiReasonText((uint8_t)last_disconnect_reason));
+  }
+}
+
+static bool loadConfig(WiFiConfig &cfg, bool &found) {
+  found = false;
+
+  Preferences prefs;
+  if (!prefs.begin(NVS_NAMESPACE, true)) {
+    return false;
+  }
+  if (!prefs.isKey("ssid")) {
+    prefs.end();
+    return false;
+  }
+
+  found = true;
+  cfg.ssid = prefs.getString("ssid", "");
+  cfg.password = prefs.getString("pass", "");
+  cfg.use_static = prefs.getBool("static", false);
+  cfg.ip = prefs.getString("ip", "");
+  cfg.gateway = prefs.getString("gw", "");
+  cfg.subnet = prefs.getString("sn", "");
+  cfg.dns = prefs.getString("dns", "");
+  prefs.end();
+  return true;
+}
+
+static bool saveConfig(const WiFiConfig &cfg) {
+  Preferences prefs;
+  if (!prefs.begin(NVS_NAMESPACE, false)) {
+    return false;
+  }
+  prefs.putString("ssid", cfg.ssid);
+  prefs.putString("pass", cfg.password);
+  prefs.putBool("static", cfg.use_static);
+  prefs.putString("ip", cfg.ip);
+  prefs.putString("gw", cfg.gateway);
+  prefs.putString("sn", cfg.subnet);
+  prefs.putString("dns", cfg.dns);
+  prefs.end();
+  return true;
+}
+
+static bool parseIP(const String &s, IPAddress &out) {
+  if (s.length() == 0 || s.length() > 15) {
+    return false;
+  }
+  return out.fromString(s.c_str());
+}
+
+static bool sameSubnet(const IPAddress &ip, const IPAddress &gateway, const IPAddress &mask) {
+  const uint32_t m = (uint32_t)mask;
+  return (((uint32_t)ip & m) == ((uint32_t)gateway & m));
+}
+
+// 连接外部 WiFi。WiFi.begin() 本身是非阻塞的，不会卡住 setup。
+static void startSTA(void) {
+  if (!wifi_cfg_active) {
+    return;
+  }
+
+  if (wifi_cfg.use_static) {
+    IPAddress ip, gateway, subnet, dns;
+    if (parseIP(wifi_cfg.ip, ip) && parseIP(wifi_cfg.gateway, gateway) &&
+        parseIP(wifi_cfg.subnet, subnet)) {
+      if (!parseIP(wifi_cfg.dns, dns)) {
+        dns = gateway;
+      }
+      if (!WiFi.config(ip, gateway, subnet, dns)) {
+        Serial.println("[WiFi] Static IP config rejected, using DHCP");
+      }
+    } else {
+      Serial.println("[WiFi] Invalid static IP in Flash, using DHCP");
+    }
+  }
+
+  Serial.printf("[WiFi] Connecting to \"%s\" (%s)...\n", wifi_cfg.ssid.c_str(),
+                wifi_cfg.use_static ? "static IP" : "DHCP");
+  WiFi.begin(wifi_cfg.ssid.c_str(), wifi_cfg.password.c_str());
+  last_retry_ms = millis();
+}
+
+// ---------------------------------------------------------------------------
+// 网页配置页面
+// ---------------------------------------------------------------------------
+static String htmlEscape(const String &in) {
+  String out;
+  out.reserve(in.length() + 16);
+  for (size_t i = 0; i < in.length(); i++) {
+    const char c = in[i];
+    switch (c) {
+      case '&': out += F("&amp;"); break;
+      case '<': out += F("&lt;"); break;
+      case '>': out += F("&gt;"); break;
+      case '"': out += F("&quot;"); break;
+      case '\'': out += F("&#39;"); break;
+      default: out += c; break;
+    }
+  }
+  return out;
+}
+
+static String buildConfigPage(void) {
+  String page;
+  page.reserve(5200);
+
+  const bool connected = (WiFi.status() == WL_CONNECTED);
+  const String ap_ip = WiFi.softAPIP().toString();
+
+  page += F(
+      "<!DOCTYPE html><html><head><meta charset=\"utf-8\">"
+      "<meta name=\"viewport\" content=\"width=device-width,initial-scale=1\">"
+      "<title>WL-PRO WiFi 设置</title><style>"
+      "body{font-family:system-ui,-apple-system,\"Segoe UI\",\"Microsoft YaHei\",Arial,sans-serif;"
+      "max-width:560px;margin:0 auto;padding:14px;background:#f4f6fb;color:#222}"
+      "h2{background:#6495ed;color:#fff;border-radius:12px;padding:12px;text-align:center;margin:0 0 12px}"
+      ".card{background:#fff;border-radius:12px;padding:14px;margin:10px 0;"
+      "box-shadow:0 1px 4px rgba(0,0,0,.08)}"
+      "label{display:block;margin:9px 0 4px;font-size:14px}"
+      "input[type=text],input[type=password]{width:100%;box-sizing:border-box;height:38px;"
+      "padding:0 9px;border:1px solid #ccd3e0;border-radius:8px;font-size:15px}"
+      ".row{display:flex;gap:10px}.row>div{flex:1}"
+      ".note{font-size:12px;color:#666;line-height:1.6;margin:8px 0 0}"
+      ".btn{display:block;width:100%;box-sizing:border-box;height:44px;line-height:44px;"
+      "text-align:center;border:0;border-radius:10px;background:#6495ed;color:#fff;"
+      "font-size:16px;margin-top:12px;text-decoration:none}"
+      ".btn.sec{background:#e9edf6;color:#334;margin-top:8px}"
+      ".ok{color:#137a37;font-weight:600}.bad{color:#b42318;font-weight:600}"
+      ".status{font-size:14px;line-height:1.8}.status b{font-weight:600}"
+      "</style></head><body><h2>WiFi 设置</h2>");
+
+  // 当前状态
+  page += F("<div class=\"card status\">");
+  page += F("<div>热点：<b>");
+  page += AP_SSID;
+  page += F("</b> / <b>");
+  page += AP_PSW;
+  page += F("</b></div>");
+  page += F("<div>热点地址：<b>http://");
+  page += ap_ip;
+  page += F("</b></div>");
+  page += F("<div>STA MAC：<b>");
+  page += WiFi.macAddress();
+  page += F("</b></div>");
+  page += F("<div>外部 WiFi：<b>");
+  page += (wifi_cfg.ssid.length() > 0) ? htmlEscape(wifi_cfg.ssid)
+                                       : String("未配置（仅热点）");
+  page += F("</b></div>");
+  page += F("<div>连接状态：<span class=\"");
+  page += connected ? F("ok") : F("bad");
+  page += F("\">");
+  if (connected) {
+    page += F("已连接");
+  } else if (wifi_cfg_active) {
+    page += F("未连接（空闲时自动重试）");
+  } else {
+    page += F("仅热点");
+  }
+  page += F("</span></div>");
+  if (connected) {
+    page += F("<div>当前 IP：<b>");
+    page += WiFi.localIP().toString();
+    page += F("</b></div>");
+    page += F("<div>信号强度：");
+    page += String((int)WiFi.RSSI());
+    page += F(" dBm</div>");
+  }
+  if (wifi_cfg.ssid.length() > 0) {
+    page += F("<div>IP 方式：<b>");
+    page += wifi_cfg.use_static ? F("静态 IP") : F("DHCP");
+    page += F("</b></div>");
+  }
+  if (!connected && wifi_cfg_active && last_disconnect_reason >= 0) {
+    page += F("<div>最近一次失败：<b>reason ");
+    page += String(last_disconnect_reason);
+    page += F("</b>（");
+    page += wifiReasonText((uint8_t)last_disconnect_reason);
+    page += F("）</div>");
+  }
+  page += F("</div>");
+
+  // 表单
+  page += F(
+      "<form method=\"POST\" action=\"/wifi/save\">"
+      "<div class=\"card\"><label>外部 WiFi 名称（SSID）</label>"
+      "<input type=\"text\" name=\"ssid\" maxlength=\"32\" value=\"");
+  page += htmlEscape(wifi_cfg.ssid);
+  page += F(
+      "\" placeholder=\"留空 = 只用热点\">"
+      "<label>密码</label>"
+      "<input type=\"password\" name=\"pass\" maxlength=\"63\" "
+      "placeholder=\"留空 = 不修改已保存密码\">"
+      "<label style=\"font-weight:normal;margin-top:10px\">"
+      "<input type=\"checkbox\" name=\"clear_pass\" value=\"1\"> "
+      "清除密码（连接开放网络）</label>"
+      "<p class=\"note\">仅支持 2.4GHz、WPA2。密码留空会保留旧密码；"
+      "换 SSID 时请重新输入密码。</p></div>");
+
+  page += F(
+      "<div class=\"card\"><label style=\"font-weight:normal\">"
+      "<input type=\"checkbox\" name=\"use_static\" value=\"1\"");
+  if (wifi_cfg.use_static) {
+    page += F(" checked");
+  }
+  page += F("> 使用静态 IP（不勾选 = DHCP）</label>");
+
+  page += F(
+      "<div class=\"row\"><div><label>IP 地址</label>"
+      "<input type=\"text\" name=\"ip\" maxlength=\"15\" value=\"");
+  page += htmlEscape(wifi_cfg.ip);
+  page += F("\" placeholder=\"192.168.1.50\"></div></div>");
+
+  page += F(
+      "<div class=\"row\"><div><label>网关</label>"
+      "<input type=\"text\" name=\"gw\" maxlength=\"15\" value=\"");
+  page += htmlEscape(wifi_cfg.gateway);
+  page += F(
+      "\" placeholder=\"192.168.1.1\"></div><div><label>子网掩码</label>"
+      "<input type=\"text\" name=\"sn\" maxlength=\"15\" value=\"");
+  page += htmlEscape(wifi_cfg.subnet);
+  page += F("\" placeholder=\"255.255.255.0\"></div></div>");
+
+  page += F(
+      "<div class=\"row\"><div><label>DNS（留空 = 网关）</label>"
+      "<input type=\"text\" name=\"dns\" maxlength=\"15\" value=\"");
+  page += htmlEscape(wifi_cfg.dns);
+  page += F(
+      "\" placeholder=\"192.168.1.1\"></div></div>"
+      "<p class=\"note\">静态 IP 必须和路由器同网段，并避开 DHCP 地址池。"
+      "热点固定使用 192.168.4.1，请避免路由器也使用 192.168.4.x 网段。</p></div>");
+
+  page += F(
+      "<button class=\"btn\" type=\"submit\">保存并重启</button>"
+      "<a class=\"btn sec\" href=\"/\">返回遥控页面</a>"
+      "<p class=\"note\" style=\"text-align:center\">保存后机器人会重启，"
+      "请先扶稳或放在支架上。</p></form>");
+  page += F("<p class=\"note\" style=\"text-align:center\">固件构建：");
+  page += WIFI_CFG_BUILD;
+  page += F("</p></body></html>");
+  return page;
+}
+
+static void sendResult(int code, const String &title, const String &message,
+                       bool refresh) {
+  if (wifi_server == nullptr) {
+    return;
+  }
+
+  String page;
+  page.reserve(900);
+  page += F(
+      "<!DOCTYPE html><html><head><meta charset=\"utf-8\">"
+      "<meta name=\"viewport\" content=\"width=device-width,initial-scale=1\">");
+  if (refresh) {
+    page += F("<meta http-equiv=\"refresh\" content=\"10;url=/wifi\">");
+  }
+  page += F(
+      "<style>body{font-family:system-ui,\"Microsoft YaHei\",Arial,sans-serif;"
+      "max-width:520px;margin:40px auto;padding:0 16px;text-align:center;line-height:1.7}"
+      "h2{color:#6495ed}a{color:#4169e1}</style></head><body><h2>");
+  page += title;
+  page += F("</h2><p>");
+  page += message;
+  page += F("</p><p><a href=\"/wifi\">返回 WiFi 设置</a>　"
+            "<a href=\"/\">遥控页面</a></p></body></html>");
+  wifi_server->send(code, "text/html; charset=utf-8", page);
+}
+
+static void handleConfigPage(void) {
+  if (wifi_server == nullptr) {
+    return;
+  }
+  wifi_server->send(200, "text/html; charset=utf-8", buildConfigPage());
+}
+
+// 直接用浏览器打开 /wifi/save（GET）时给出提示，而不是只回一个 404
+static void handleSaveGet(void) {
+  if (wifi_server == nullptr) {
+    return;
+  }
+  sendResult(405, "请求方式不对",
+             "请返回 WiFi 设置页面，填写后点击“保存并重启”按钮。", false);
+}
+
+static void handleVersion(void) {
+  if (wifi_server == nullptr) {
+    return;
+  }
+  String msg = F("wifi_manager build: ");
+  msg += WIFI_CFG_BUILD;
+  wifi_server->send(200, "text/plain; charset=utf-8", msg);
+}
+
+static void handleNotFound(void) {
+  if (wifi_server == nullptr) {
+    return;
+  }
+  const HTTPMethod m = wifi_server->method();
+  const char *name = (m == HTTP_GET)     ? "GET"
+                     : (m == HTTP_POST)  ? "POST"
+                                         : "OTHER";
+  Serial.printf("[HTTP] 404 %s %s\n", name, wifi_server->uri().c_str());
+
+  String msg = F("Not found: ");
+  msg += wifi_server->uri();
+  msg += F(" (method ");
+  msg += name;
+  msg += F(")");
+  wifi_server->send(404, "text/plain; charset=utf-8", msg);
+}
+
+static void handleSave(void) {
+  if (wifi_server == nullptr) {
+    return;
+  }
+
+  Serial.printf("[WiFi] /wifi/save request, method=%d, args=%d\n",
+                (int)wifi_server->method(), wifi_server->args());
+
+  String ssid = wifi_server->arg("ssid");
+  ssid.trim();
+  if (ssid.length() > 32) {
+    sendResult(400, "保存失败", "SSID 不能超过 32 个字节。", false);
+    return;
+  }
+
+  const bool clear_pass = wifi_server->hasArg("clear_pass");
+  String pass = wifi_server->arg("pass");
+  if (clear_pass) {
+    pass = "";
+  } else if (pass.length() == 0) {
+    // 留空表示沿用 Flash 里的旧密码
+    pass = wifi_cfg.password;
+  }
+
+  if (pass.length() > 63) {
+    sendResult(400, "保存失败", "WiFi 密码不能超过 63 个字符。", false);
+    return;
+  }
+
+  if (ssid.length() > 0 && !clear_pass && pass.length() == 0 &&
+      ssid != wifi_cfg.ssid) {
+    sendResult(400, "保存失败",
+               "更换 SSID 时请重新输入密码；如果是开放网络，请勾选“清除密码”。",
+               false);
+    return;
+  }
+
+  if (ssid.length() > 0 && pass.length() > 0 && pass.length() < 8) {
+    sendResult(400, "保存失败",
+               "WPA2 密码至少 8 位；开放网络请勾选“清除密码”。", false);
+    return;
+  }
+
+  const bool use_static = wifi_server->hasArg("use_static");
+  String ip = wifi_server->arg("ip");
+  String gateway = wifi_server->arg("gw");
+  String subnet = wifi_server->arg("sn");
+  String dns = wifi_server->arg("dns");
+  ip.trim();
+  gateway.trim();
+  subnet.trim();
+  dns.trim();
+
+  IPAddress ip_addr, gateway_addr, subnet_addr, dns_addr;
+  if (use_static && ssid.length() > 0) {
+    if (!parseIP(ip, ip_addr) || (uint32_t)ip_addr == 0) {
+      sendResult(400, "保存失败", "静态 IP 地址无效。", false);
+      return;
+    }
+    if (!parseIP(gateway, gateway_addr) || (uint32_t)gateway_addr == 0) {
+      sendResult(400, "保存失败", "网关地址无效。", false);
+      return;
+    }
+    if (!parseIP(subnet, subnet_addr) || (uint32_t)subnet_addr == 0) {
+      sendResult(400, "保存失败", "子网掩码无效。", false);
+      return;
+    }
+    if (dns.length() == 0) {
+      dns = gateway;
+      dns_addr = gateway_addr;
+    } else if (!parseIP(dns, dns_addr) || (uint32_t)dns_addr == 0) {
+      sendResult(400, "保存失败", "DNS 地址无效。", false);
+      return;
+    }
+    if (!sameSubnet(ip_addr, gateway_addr, subnet_addr)) {
+      sendResult(400, "保存失败",
+                 "静态 IP 和网关不在同一网段，请检查 IP/网关/子网掩码。", false);
+      return;
+    }
+    if (sameSubnet(ip_addr, AP_IP, AP_SUBNET)) {
+      sendResult(400, "保存失败",
+                 "静态 IP 不能使用热点的 192.168.4.x 网段。", false);
+      return;
+    }
+  }
+
+  WiFiConfig cfg;
+  cfg.ssid = ssid;
+  cfg.password = pass;
+  cfg.use_static = use_static;
+  cfg.ip = ip;
+  cfg.gateway = gateway;
+  cfg.subnet = subnet;
+  cfg.dns = dns;
+
+  if (!saveConfig(cfg)) {
+    sendResult(500, "保存失败", "写入 Flash 失败，请重试。", false);
+    return;
+  }
+
+  // 让状态页立即反映新配置（随后会重启）
+  wifi_cfg = cfg;
+  wifi_cfg_active = (cfg.ssid.length() > 0);
+
+  String message;
+  if (cfg.ssid.length() == 0) {
+    message = "已保存。机器人将重启，之后只使用热点 WLROBOT。";
+  } else if (cfg.use_static) {
+    message = "已保存。机器人将在约 1 秒后重启，并尝试连接 \"";
+    message += htmlEscape(cfg.ssid);
+    message += "\"，静态 IP 为 ";
+    message += cfg.ip;
+    message += "。";
+  } else {
+    message = "已保存。机器人将在约 1 秒后重启，并尝试连接 \"";
+    message += htmlEscape(cfg.ssid);
+    message += "\"（DHCP）。";
+  }
+  message += " 如果外部 WiFi 连不上，热点 WLROBOT 仍然可用"
+             "（http://192.168.4.1）。";
+
+  sendResult(200, "保存成功", message, true);
+  restart_at_ms = millis() + 1000;
+}
+
+void WiFi_RegisterWebHandlers(WebServer &server) {
+  wifi_server = &server;
+  server.on("/wifi", HTTP_GET, handleConfigPage);
+  server.on("/wifi/", HTTP_GET, handleConfigPage);
+  server.on("/wifi/version", HTTP_GET, handleVersion);
+  server.on("/wifi/save", HTTP_POST, handleSave);
+  server.on("/wifi/save", HTTP_GET, handleSaveGet);
+  server.onNotFound(handleNotFound);
+}
+
+// ---------------------------------------------------------------------------
+// 初始化与主循环
+// ---------------------------------------------------------------------------
+void WiFi_Init(void) {
+  Serial.printf("[WiFi] wifi_manager build: %s\n", WIFI_CFG_BUILD);
+  WiFi.mode(WIFI_AP_STA);
+  WiFi.onEvent(onWiFiEvent);
+  WiFi.setHostname("wlrobot");
+  WiFi.setAutoReconnect(true);
+
+  WiFi.softAPConfig(AP_IP, AP_GATEWAY, AP_SUBNET);
+  if (!WiFi.softAP(AP_SSID, AP_PSW)) {
+    Serial.println("[WiFi] softAP start failed");
+  }
+
+  Serial.printf("\n[WiFi] AP \"%s\" / \"%s\" -> http://%s/\n", AP_SSID, AP_PSW,
+                WiFi.softAPIP().toString().c_str());
+  Serial.printf("[WiFi] Settings page: http://%s/wifi\n",
+                WiFi.softAPIP().toString().c_str());
+  Serial.printf("[WiFi] STA MAC: %s, AP MAC: %s\n", WiFi.macAddress().c_str(),
+                WiFi.softAPmacAddress().c_str());
+
+  bool found = false;
+  loadConfig(wifi_cfg, found);
+  if (!found) {
+    // 首次烧录：沿用原代码的默认外部 WiFi，之后可在 /wifi 页面修改
+    wifi_cfg.ssid = DEFAULT_STA_SSID;
+    wifi_cfg.password = DEFAULT_STA_PASS;
+    wifi_cfg.use_static = false;
+    wifi_cfg.ip = "";
+    wifi_cfg.gateway = "";
+    wifi_cfg.subnet = "";
+    wifi_cfg.dns = "";
+    Serial.println("[WiFi] No saved config, using default STA credentials");
+  }
+
+  wifi_cfg_active = (wifi_cfg.ssid.length() > 0);
+  if (wifi_cfg_active) {
+    startSTA();
+  } else {
+    // 清掉 WiFi 驱动里缓存的旧 AP，避免“仅热点”模式下又自动连上旧网络
+    WiFi.disconnect(false, true);
+    Serial.println("[WiFi] No external WiFi configured, AP only");
+  }
+}
+
+void WiFi_Loop(void) {
+  // 配置保存后延时重启，避免在 Web 回调里阻塞平衡控制
+  if (restart_at_ms != 0 && (int32_t)(millis() - restart_at_ms) >= 0) {
+    Serial.println("[WiFi] Rebooting to apply settings...");
+    delay(50);
+    ESP.restart();
+  }
+
+  const wl_status_t status = WiFi.status();
+
+  if (status == WL_CONNECTED) {
+    if (!sta_was_connected) {
+      sta_was_connected = true;
+      Serial.printf("[WiFi] STA connected: \"%s\", IP %s\n",
+                    WiFi.SSID().c_str(), WiFi.localIP().toString().c_str());
+      Serial.printf("[WiFi] Remote UI: http://%s/  Settings: http://%s/wifi\n",
+                    WiFi.localIP().toString().c_str(),
+                    WiFi.localIP().toString().c_str());
+    }
+    return;
+  }
+
+  if (sta_was_connected) {
+    sta_was_connected = false;
+    Serial.println("[WiFi] STA disconnected; reconnecting in background "
+                   "(AP still available)");
+  }
+
+  // 外部 WiFi 未连接时后台重试。即使手机正连着热点也照常重试，
+  // 否则用户连上 AP 查看状态时反而永远不会重连。
+  if (wifi_cfg_active && (millis() - last_retry_ms > 15000)) {
+    last_retry_ms = millis();
+    Serial.printf("[WiFi] retrying \"%s\"...\n", wifi_cfg.ssid.c_str());
+    WiFi.reconnect();
+  }
+}

3.Software/wl_pro_robot/wifi_manager.h

@@ -23,6 +23,13 @@
 #pragma once
 
 #include <WiFi.h>
+#include <WebServer.h>
 
-void WiFi_SetAP(void);
+// 启动 AP 热点，并按 Flash 里保存的配置异步连接外部 WiFi（STA）
-void set_sta_wifi(); //STA模式接入WiFi网络
+void WiFi_Init(void);
+
+// 在主循环里调用：处理重连、串口状态打印、延时重启
+void WiFi_Loop(void);
+
+// 注册 /wifi 配置页面（需要在 webserver.begin() 之前调用）
+void WiFi_RegisterWebHandlers(WebServer &server);

3.Software/wl_pro_robot/wl_pro_robot.ino

@@ -34,7 +34,7 @@
 #include <FS.h>
 #include "basic_web.h"
 #include "robot.h"
-#include "wifi.h"
+#include "wifi_manager.h"
 #include "esp_adc_cal.h"
 
 /************实例定义*************/
@@ -95,7 +95,7 @@ int joystick_value[2];
 SMS_STS sms_sts;
 
 //MPU6050实例
-MPU6050 mpu6050(I2Ctwo);
+MPU6050 mpu6050(I2Cone);
 
 /************参数定义*************/
 #define pi 3.1415927
@@ -135,6 +135,9 @@ int jump_flag = 0;              //跳跃时段标识
 float leg_position_add = 0;     //roll轴平衡控制量
 int uncontrolable = 0;          //机身倾角过大导致失控
 
+//【调试】测试平衡方向时置 true，屏蔽 YAW 差速，避免转向干扰轮子转向判断；测试完成改回 false
+const bool DEBUG_DISABLE_YAW = false;
+
 //电压检测
 uint16_t bat_check_num = 0;
 int BAT_PIN = 35;    // select the input pin for the ADC
@@ -151,11 +154,11 @@ void setup() {
   Serial.begin(115200);//通讯串口
   Serial2.begin(1000000);//腿部sts舵机
 
-  //Wifi初始化
+  //Wifi初始化：AP热点 + 外部WiFi（STA）
-  WiFi_SetAP();
+  WiFi_Init();
-  // set_sta_wifi();      // ESP-01S STA模式接入WiFi网络
+  WiFi_RegisterWebHandlers(webserver);
-  webserver.begin();
   webserver.on("/", HTTP_GET, basicWebCallback);
+  webserver.begin();
   websocket.begin();
   websocket.onEvent(webSocketEventCallback);
 
@@ -233,6 +236,9 @@ void setup() {
   motor2.init();
   motor2.initFOC();
 
+  //【调试】初始化位移零点，避免 distance_zeropoint=-256 造成的启动冲击
+  distance_zeropoint = -0.5f * (motor1.shaft_angle + motor2.shaft_angle);
+
   // 映射电机到commander
     command.add('A', StabAngle, "pid angle");
     command.add('B', StabGyro, "pid gyro");
@@ -258,13 +264,33 @@ void loop() {
   bat_check();        //电压检测
   web_loop();         //Web数据更新
   mpu6050.update();   //IMU数据更新
+
+  //勾选 Robot Go! 的上升沿：以当前姿态作为新的航向/位移零点
+  static bool go_last = false;
+  if (wrobot.go && !go_last) {
+    YAW_angle_total = 0;
+    YAW_angle_last  = mpu6050.getAngleZ();
+    distance_zeropoint = -0.5f * (motor1.shaft_angle + motor2.shaft_angle);
+  }
+  go_last = wrobot.go;
+
   lqr_balance_loop(); //lqr自平衡控制
+
+  //【调试】每100ms输出一次平衡状态
+  static uint32_t t_dbg = 0;
+  if (millis() - t_dbg > 100) {
+    t_dbg = millis();
+    Serial.printf("ang=%.2f gyro=%.2f zp=%.2f dist=%.2f u=%.2f spd=%.2f unc=%d\n",
+                  LQR_angle, LQR_gyro, angle_zeropoint, distance_control, LQR_u, LQR_speed, uncontrolable);
+  }
+
   yaw_loop();         //yaw轴转向控制
   leg_loop();         //腿部动作控制
   
   //将自平衡计算输出转矩赋给电机
-  motor1.target = (-0.5)*(LQR_u + YAW_output);
+  const float yaw_out = DEBUG_DISABLE_YAW ? 0.0f : YAW_output;
-  motor2.target = (-0.5)*(LQR_u - YAW_output);
+  motor1.target = (-0.5)*(LQR_u + yaw_out);
+  motor2.target = (-0.5)*(LQR_u - yaw_out);
 
   //倒地失控后关闭输出
   if( abs(LQR_angle) > 25.0f  )
@@ -370,6 +396,7 @@ void lqr_balance_loop(){
     LQR_u = pid_lqr_u(LQR_u);//补偿小转矩非线性
     //Serial.println(LQR_u);
     angle_zeropoint -= pid_zeropoint(lpf_zeropoint(distance_control));//重心自适应
+    angle_zeropoint = constrain(angle_zeropoint, -10.0f, 10.0f);       //限幅，防止平衡点跑飞
    }
   else
   {
@@ -467,6 +494,7 @@ void yaw_loop(){
 
 //Web数据更新
 void web_loop(){
+  WiFi_Loop();    //WiFi状态维护/后台重连
   webserver.handleClient();
   websocket.loop();
   rp.spinOnce();//更新web端回传的控制信息

README.md

@@ -46,9 +46,10 @@ QQ group (unofficial): 991727288
 * 2. After turning on the switch, a red light on the main board will indicate that the power is on.
 * 3. Next, the wheels will start FOC motor initialization, with the wheels moving slightly and the legs starting to move.
 * 4. If the battery is sufficient, the blue LED on the main control board will light up. If it does not light up, charging is needed.
-* 5. After this process is complete, press the EN button on the main control board to restart, and you can then connect to the robot's WiFi network starting with WL. The password is the lowercase WiFi name.
+* 5. After this process is complete, press the EN button on the main control board to restart. Connect to the robot's WiFi network `WLROBOT` (password `12345678`).
-* 6. Open a browser and go to the URL 192.168.1.11. The remote control interface is compatible with Android, iOS, Windows, Linux, macOS, etc., and it is recommended to use Chrome or Firefox.
+* 6. Open `http://192.168.4.1` in a browser. The remote control interface is compatible with Android, iOS, Windows, Linux, macOS, etc., and it is recommended to use Chrome or Firefox.
-* 7. Manually stabilize the robot, with the wheels slightly touching the ground. Click the "Robot go!" button on the web page, and the robot will stand up. You can then control the robot's movement using the joystick.
+* 7. To use an external WiFi router, open "WiFi 设置 / WiFi Settings" on the page, enter the 2.4 GHz SSID/password (static IP is optional), and save. The robot will reboot and join that network. The `WLROBOT` AP remains available as a fallback.
+* 8. Manually stabilize the robot, with the wheels slightly touching the ground. Click the "Robot go!" button on the web page, and the robot will stand up. You can then control the robot's movement using the joystick.
 
 #### Open Source Contributors
 

README_CN.md

@@ -51,9 +51,10 @@
 * 2.打开开关后，主板上红灯亮起，表示电源接通。
 * 3.其次，双轮开始进行 FOC 电机初始化，两轮依次微动，腿部开始运动。
 * 4.如果电量充足，主控板上的蓝色 LED 会亮起，初始化结束未亮起表示需要充电。
-* 5.上述过程结束后，按一下主控板上的 EN 键重启一下，就可以连接机器人发出的以 WL 开头的 WiFii，密码是小写的 WiFi 名字。
+* 5.上述过程结束后，按一下主控板上的 EN 键重启一下，连接机器人发出的 `WLROBOT` 热点，密码为 `12345678`。
-* 6.打开浏览器，进入网址 192.168.1.11，遥控界面适用于 Android、ios、windows、linux、macos 等操作系统，推荐使用 chrome 或火狐浏览器。
+* 6.打开浏览器，进入网址 `http://192.168.4.1`，遥控界面适用于 Android、ios、windows、linux、macos 等操作系统，推荐使用 chrome 或火狐浏览器。
-* 7.手动扶正机器人，轮子微微接触地面，点击网页端的 Robot go!按钮机器人即可站立，此时可通过摇杆控制机器人的运动。
+* 7.如果想接入外部路由器，在网页上打开“WiFi 设置”，填入 2.4GHz 的 SSID/密码（可选静态 IP），保存后机器人会重启并连接该网络；`WLROBOT` 热点仍然保留，作为连不上时的备用入口。
+* 8.手动扶正机器人，轮子微微接触地面，点击网页端的 Robot go!按钮机器人即可站立，此时可通过摇杆控制机器人的运动。
 
 
 #### 开源贡献者