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/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_SetAP();
-  // set_sta_wifi();      // ESP-01S STA模式接入WiFi网络
-  webserver.begin();
+  //Wifi初始化：AP热点 + 外部WiFi（STA）
+  WiFi_Init();
+  WiFi_RegisterWebHandlers(webserver);
   webserver.on("/", HTTP_GET, basicWebCallback);
+  webserver.begin();
   websocket.begin();
   websocket.onEvent(webSocketEventCallback);
 
@@ -229,10 +232,13 @@ void setup() {
   motor2.useMonitoring(Serial);
   //电机初始化
   motor1.init();
-  motor1.initFOC(); 
+  motor1.initFOC();
   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);
-  motor2.target = (-0.5)*(LQR_u - YAW_output);
+  const float yaw_out = DEBUG_DISABLE_YAW ? 0.0f : 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,7 +396,8 @@ 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
   {
     pid_lqr_u.error_prev = 0; //输出积分清零
@@ -467,6 +494,7 @@ void yaw_loop(){
 
 //Web数据更新
 void web_loop(){
+  WiFi_Loop();    //WiFi状态维护/后台重连
   webserver.handleClient();
   websocket.loop();
   rp.spinOnce();//更新web端回传的控制信息