稳定版

README.md

@@ -10,8 +10,6 @@
 2025-08-16
 对几种运动中倒地的极限状态进行优化，实现自平衡起身。
 
-2025-08-17
-优化跳跃功能，可以支持连续跳跃，不倒地。
 
 世博开源轮足优化版-TOMMY版本项目地址：
 https://gitee.com/myperworld/wrobot

Shibo-main-TOMMY/platformio.ini

@@ -12,8 +12,11 @@
 platform = espressif32
 board = esp32dev
 framework = arduino
-
 monitor_speed = 115200
 monitor_echo = yes
 monitor_rts = 0
 monitor_dtr = 0
+lib_deps = adafruit/Adafruit SSD1306@^2.5.7
+build_flags = 
+	-D WIRE_SCL=15
+	-D WIRE_SDA=4

Shibo-main-TOMMY/src/main.cpp

@@ -69,8 +69,8 @@ float gyro_control = 0;
 float speed_control = 0;
 float distance_control = 0;
 float LQR_u = 0;
-float angle_zeropoint = 5;  // 设定默认俯仰角度，向前到正，向后倒负
+float angle_zeropoint = 5;  // 5 设定默认俯仰角度，向前到正，向后倒负
-float distance_zeropoint = 1; // 轮部位移零点偏置
+float distance_zeropoint =  0.5f; // 轮部位移零点偏置
 
 // YAW轴控制数据
 float YAW_gyro = 0;
@@ -138,15 +138,6 @@ bool lastbuttonLB = false;
 void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
   memcpy(&remoteData, incomingData, sizeof(remoteData));
 
-  // 跳跃状态不接受指令
-  if(jump_flag > 0){
-    wrobot.dir = 4; // 不跳跃
-    wrobot.joyx = 0;
-    wrobot.joyy = 0;
-    wrobot.roll = 0;
-    return;
-  }
-
   // 摇杆输出范围0-255，转换到 -100 到 100
   // roll 左右摇摆
   int convertedJ1PotX = (remoteData.j1PotX - 127.5) * (30.0 / 127.5); // 转到 -30 到 30
@@ -160,8 +151,6 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
   // 取反按钮值，使得默认显示 0 false，按下显示 1 true
   int convertedbuttonLB = !remoteData.buttonLB; // 顶部左
   int convertedbuttonRB = !remoteData.buttonRB; // 顶部右
-  // int convertedbuttonR1 = !remoteData.buttonR1; // 底部右
-  // int convertedbuttonR2 = !remoteData.buttonR2; // 底部左
 
   // 跳跃
   if (convertedbuttonRB) { // 1=true，0=false
@@ -169,7 +158,6 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
   }else{
     wrobot.dir = 4; // 不跳跃
   }
-  // Serial.print(convertedbuttonRB);
 
   // led蓝灯爆闪
   if (lastbuttonLB && !convertedbuttonLB) {
@@ -229,10 +217,10 @@ void setup()
   ACC[1] = 30;
   Speed[0] = 400;
   Speed[1] = 400;
-  // Position[0] = 2048;
+  Position[0] = 2048;
-  // Position[1] = 2048;
+  Position[1] = 2048;
-  Position[0] = SERVO1_MIN;
+  // Position[0] = SERVO1_MIN;
-  Position[1] = SERVO2_MIN;
+  // Position[1] = SERVO2_MIN;
   // 舵机(ID1/ID2)以最高速度V=2400步/秒，加速度A=50(50*100步/秒^2)，运行至各自的Position位置
   sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
 
@@ -296,43 +284,56 @@ void setup()
   motor2.initFOC();
 
   resetToDefault(); // 重置到默认参数状态
+
   delay(500);
 }
 
 void loop()
 {
   
-  bat_check();        // 电压检测
+  bat_check();        // 电压检测 <1ms
-  web_loop();         // Web数据更新
+  web_loop();         // Web数据更新 <2ms 
-  mpu6050.update();   // IMU数据更新
+  mpu6050.update();   // IMU数据更新  <1ms
-  lqr_balance_loop(); // lqr自平衡控制，更新LQR_u
+  lqr_balance_loop(); // lqr自平衡控制，更新LQR_u ★ 关键实时控制 2-3ms
-  yaw_loop();         // yaw轴转向控制，更新YAW_output
+  yaw_loop();         // yaw轴转向控制，更新YAW_output ★ 关键实时控制 1-2ms
-  leg_loop();         // 腿部动作控制，控制舵机SyncWritePosEx
+  leg_loop();         // 腿部动作控制，控制舵机SyncWritePosEx，包括判断jump_loop 舵机控制 1ms
 
   // 记录上一次的遥控数据数据
   wrobot.dir_last = wrobot.dir;
   wrobot.joyx_last = wrobot.joyx;
   wrobot.joyy_last = wrobot.joyy;
 
-  // 将自平衡计算输出转矩赋给电机
+  /* 将自平衡计算输出转矩赋给电机
-  // motor.target 负数小车向前转，正数小车向后转，
+  motor.target 负数小车向前转，正数小车向后转，
+  LQR_u 越大，电机输出的转矩越大，电机向后转，默认0-8V
+  LQR_u = angle_control + gyro_control + distance_control + speed_control
+
+   angle_control = pid_angle(LQR_angle - angle_zeropoint) //LQR_u与实际LQR_angle正相关
+                 = pid_angle( mpu6050.getAngleY() - angle_zeropoint) //LQR_u与实际pitch正相关
+   gyro_control = pid_gyro(LQR_gyro) //LQR_u与实际LQR_gyro正相关
+                = pid_gyro( mpu6050.getGyroY() ) //LQR_u与实际pitch 角速度 正相关
+    
+    所以这里LQR_distance和LQR_speed的计算乘以-0.5,不能单看负号
+   distance_control = pid_distance(LQR_distance - distance_zeropoint) //LQR_u与电LQR_distance正相关 
+                    = pid_distance( (-0.5)*(motor1.shaft_angle + motor2.shaft_angle) - distance_zeropoint)  
+                    //在平衡车中，通常设定电机向后转（使车前进）为负，所以LQR_u与电机实际shaft_angle角度正相关，和负号没关系
+   
+    speed_control = pid_speed(LQR_speed - 0.1 * lpf_joyy(wrobot.joyy)) // 实际速度-目标速度
+                  //LQR_speed代表实际速度，LQR_u与实际LQR_speed速度正相关
+                  = pid_speed( (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity) - 0.1 * lpf_joyy(wrobot.joyy)) 
+                  //LQR_u与实际shaft_velocity速度正相关
+   */
   motor1.target = (-0.5) * (LQR_u + YAW_output); // 负数小车向前，正数小车向后，取
   motor2.target = (-0.5) * (LQR_u - YAW_output);
 
   // 查看角度信息, 屁股向后倒 -33~-34度,舵机2048的时候-42度，头向前到底 75度，完全垂直88度
   //  Serial.println(LQR_angle);
-  
-  // 检测跳跃状态
-  // if(jump_flag>0){
-  //   Serial.println(jump_flag);
-  // }
-   
 
   // 检测小车是否向后马上要倒地了，在中间过程重置所有参数
   loopCounter++;
-  // loopCounter作用：在刚开机的800次loop循环，不能进入resetToDefault()函数
+  // loopCounter作用：在期初的800次循环内视为电源开通后第一次自平衡，不能进入resetToDefault()函数重置参数
-  // resetToDefaultCounter作用：防止小车倒地后某个角度，陷入循环，motor.target一直等于0，就躺地上不动了
+  // resetToDefaultCounterz作用：防止小车倒地后某个角度，陷入循环，motor.target一直=0
-  if ((LQR_angle < -35.0f || LQR_angle > 70.0f )&& loopCounter > 800 && resetToDefaultCounter < 130) {
+  if ((LQR_angle < -35.0f || LQR_angle > 70.0f ) && loopCounter > 800 && resetToDefaultCounter < 130) {
       resetToDefault();
       resetToDefaultCounter++;
       motor1.target = 0;
@@ -346,9 +347,13 @@ void loop()
 
   // target目标电压 0~8V, 0为不转, 8为最大转速
   // Serial.print("LQR_angle:");
-  // Serial.print(LQR_angle);
+  // Serial.println(LQR_angle);
-  // Serial.print(",motor1.target:");
+  // // Serial.print(",loopCounter:");
-  // Serial.print(motor1.target);
+  // // Serial.print(loopCounter);
+  // Serial.print(",resetToDefaultCounter:");
+  // Serial.print(resetToDefaultCounter);
+  // // Serial.print(",motor1.target:");
+  // // Serial.print(motor1.target);
   // Serial.print(",motor2.target:");
   // Serial.println(motor2.target);
 
@@ -360,8 +365,6 @@ void loop()
   motor1.move();
   motor2.move();
 
-  jump_loop(); // 跳跃控制
-
 } // loop()
 
 // lqr自平衡控制
@@ -379,31 +382,9 @@ void lqr_balance_loop()
         // + LQR_k4*LQR_speed;
 
   // 给负值是因为按照当前的电机接线，正转矩驱动小车电机向后转
-
+  // 编码器反馈shaft_angle角度，shaft_velocity速度
-  /*
+  // 不操作前后移动，譬如刚开机，车轮不动的，shaft_angle和shaft_velocity 都是零
-  LQR_u 越大，电机输出的转矩越大，电机向后转，默认0-8V
+  // 所以，不操作前后移动，LQR_distance 和 LQR_speed趋近于0，只有角度控制和角速度控制
-  LQR_u = angle_control + gyro_control + distance_control + speed_control
-
-  编码器反馈shaft_angle角度，shaft_velocity速度
-  不操作前后移动，譬如刚开机，车轮不动的，shaft_angle和shaft_velocity 都是零
-  所以，不操作前后移动，LQR_distance 和 LQR_speed趋近于0，只有角度控制和角速度控制
-
-   angle_control = pid_angle(LQR_angle - angle_zeropoint) //LQR_u与实际LQR_angle正相关
-                 = pid_angle( mpu6050.getAngleY() - angle_zeropoint) //LQR_u与实际pitch正相关
-   gyro_control = pid_gyro(LQR_gyro) //LQR_u与实际LQR_gyro正相关
-                = pid_gyro( mpu6050.getGyroY() ) //LQR_u与实际pitch 角速度 正相关
-    
-    所以这里LQR_distance和LQR_speed的计算乘以-0.5,不能单看负号
-   distance_control = pid_distance(LQR_distance - distance_zeropoint) //LQR_u与电LQR_distance正相关 
-                    = pid_distance( (-0.5)*(motor1.shaft_angle + motor2.shaft_angle) - distance_zeropoint)  
-                    //在平衡车中，通常设定电机向后转（使车前进）为负，所以LQR_u与电机实际shaft_angle角度正相关，和负号没关系
-   
-    speed_control = pid_speed(LQR_speed - 0.1 * lpf_joyy(wrobot.joyy)) // 实际速度-目标速度
-                  //LQR_speed代表实际速度，LQR_u与实际LQR_speed速度正相关
-                  = pid_speed( (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity) - 0.1 * lpf_joyy(wrobot.joyy)) 
-                  //LQR_u与实际shaft_velocity速度正相关
-  */
-
   // 在平衡车中，通常设定电机向后转（使车前进）为负，所以这里LQR_distance和LQR_speed的计算乘以-0.5
   LQR_distance = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); // 两个电机的旋转角度（shaft_angle）（是位置量）的平均值
   LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity); // 两个电机角速度（shaft_velocity）的平均值，并取反
@@ -515,6 +496,8 @@ void leg_loop()
       return; // 跳过后续控制
   }
 
+  jump_loop();
+
   if (jump_flag == 0) // 不处于跳跃状态
   {
     // 舵机加速度
@@ -589,9 +572,8 @@ void leg_loop()
 void jump_loop()
 { // dir: 4 = STOP, 5 = jump 上次动作是跳跃5，本次是停止4，才开始跳，因为可能连续发送了很多5跳跃指令，只执行最后一次
   // 跳跃，腿部以最快速度伸到最长
-  if ((wrobot.dir == 5) && (jump_flag == 0))
+  if ((wrobot.dir_last == 5) && (wrobot.dir == 4) && (jump_flag == 0))
   { 
-
     // 跳跃开始
     ACC[0] = 0;
     ACC[1] = 0;
@@ -614,10 +596,8 @@ void jump_loop()
       ACC[1] = 0;
       Speed[0] = 0;
       Speed[1] = 0;
-      // Position[0] = 2048 + 12 + 8.4 * (35 - 32); // 降低腿部高度，即车重心
+      Position[0] = 2048 + 12 + 8.4 * (35 - 32); // 降低腿部高度，即车重心
-      // Position[1] = 2048 - 12 - 8.4 * (35 - 32); 
+      Position[1] = 2048 - 12 - 8.4 * (35 - 32);
-      Position[0] = SERVO1_MIN + 150; // 收缩腿
-      Position[1] = SERVO2_MIN - 150;
       sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
 
       jump_flag = 40;
@@ -768,18 +748,14 @@ void blinkLed() {
 
 // 重置到默认参数状态，用于自平衡起立
 void resetToDefault() {
-    // LQR自平衡控制器参数
+
-    LQR_angle = 0;
-    LQR_gyro = 0;
-    angle_control = 0;
-    gyro_control = 0;
     angle_zeropoint = 5;
 
     LQR_speed = 0;
     LQR_distance = 0;
     speed_control = 0;
     distance_control = 0;
-    distance_zeropoint = 1;
+    distance_zeropoint =  0.5f;
     LQR_u = 0;
     
     // YAW轴控制数据
@@ -799,6 +775,5 @@ void resetToDefault() {
 
     pid_lqr_u.error_prev = 0;
 
-    
+  
-
 }

Shibo-main-TOMMY/test/main jump.cpp

@@ -0,0 +1,804 @@
+// -----------------------------------------------------------------------------
+// Copyright (c) 2024 Mu Shibo
+// Modify by Tommy 2025.8.11
+// -----------------------------------------------------------------------------
+
+#include <esp_now.h>
+#include <WiFi.h>
+#include "main.h"
+
+/************实例定义*************/
+
+// 电机实例
+BLDCMotor motor1 = BLDCMotor(7);
+BLDCMotor motor2 = BLDCMotor(7);
+BLDCDriver3PWM driver1 = BLDCDriver3PWM(32, 33, 25, 22);
+BLDCDriver3PWM driver2 = BLDCDriver3PWM(26, 27, 14, 12);
+
+// 编码器实例
+TwoWire I2Cone = TwoWire(0);
+TwoWire I2Ctwo = TwoWire(1);
+MagneticSensorI2C sensor1 = MagneticSensorI2C(AS5600_I2C);
+MagneticSensorI2C sensor2 = MagneticSensorI2C(AS5600_I2C);
+
+// PID控制器实例
+PIDController pid_angle(1, 0, 0, 100000, 8); // pitch角度PID使其趋于零点
+PIDController pid_gyro(0.06, 0, 0, 100000, 8); // 角速度PID使其趋于零点
+PIDController pid_distance(0.5, 0, 0, 100000, 8);// 位移PID使其趋于零点
+PIDController pid_speed(0.7, 0, 0, 100000, 8);// 速度PID使其趋于目标速度
+PIDController pid_yaw_angle(1.0, 0, 0, 100000, 8);
+PIDController pid_yaw_gyro(0.04, 0, 0, 100000, 8);
+PIDController pid_lqr_u(1, 15, 0, 100000, 8);
+PIDController pid_zeropoint(0.002, 0, 0, 100000, 4);
+PIDController pid_roll_angle(8, 0, 0, 100000, 450);
+
+// 低通滤波器实例
+LowPassFilter lpf_joyy(0.2);
+LowPassFilter lpf_zeropoint(0.1);
+LowPassFilter lpf_roll(0.3);
+
+// WebServer实例
+WebServer webserver;                               // server服务器
+WebSocketsServer websocket = WebSocketsServer(81); // 定义一个webSocket服务器来处理客户发送的消息
+RobotProtocol rp(20);
+
+// STS舵机实例
+SMS_STS sms_sts;
+
+// MPU6050实例
+MPU6050 mpu6050(I2Ctwo);
+
+/************参数定义*************/
+#define pi 3.1415927
+
+// 舵机位置限制参数
+#define SERVO1_MIN 1900 // 舵机1最低 从小车正面看右手
+#define SERVO1_MAX 2400 // 舵机1最高
+#define SERVO2_MIN 2300 // 舵机2最低 从小车正面看左手(注意大小值与舵机1是相反的)
+#define SERVO2_MAX 1800 // 舵机2最高
+
+#define  LEG_HEIGHT_BASE 32  // 腿部默认高度基准值
+
+// LQR自平衡控制器参数
+float LQR_angle = 0;
+float LQR_gyro = 0;
+float LQR_speed = 0;
+float LQR_distance = 0;
+float angle_control = 0;
+float gyro_control = 0;
+float speed_control = 0;
+float distance_control = 0;
+float LQR_u = 0;
+float angle_zeropoint = 5;  // 设定默认俯仰角度，向前到正，向后倒负
+float distance_zeropoint = 1; // 轮部位移零点偏置
+
+// YAW轴控制数据
+float YAW_gyro = 0;
+float YAW_angle = 0;
+float YAW_angle_last = 0;
+float YAW_angle_total = 0;
+float YAW_angle_zero_point = -10;
+float YAW_output = 0;
+
+
+// 腿部舵机控制数据
+byte ID[2];
+s16 Position[2];
+u16 Speed[2];
+byte ACC[2];
+
+// 逻辑处理标志位
+float robot_speed = 0;         // 记录当前轮部转速
+float robot_speed_last = 0;    // 记录上一时刻的轮部转速
+int wrobot_move_stop_flag = 0; // 记录摇杆停止的标识
+int jump_flag = 0;             // 跳跃时段标识
+float leg_position_add = 0;    // roll轴平衡控制量
+
+// 电压检测
+uint16_t bat_check_num = 0;
+int BAT_PIN = 35; // select the input pin for the ADC
+static esp_adc_cal_characteristics_t adc_chars;
+static const adc1_channel_t channel = ADC1_CHANNEL_7;
+static const adc_bits_width_t width = ADC_WIDTH_BIT_12;
+static const adc_atten_t atten = ADC_ATTEN_DB_11;
+static const adc_unit_t unit = ADC_UNIT_1;
+
+int loopCounter = 0; // loop函数执行计数
+int resetToDefaultCounter = 0; // resetToDefault执行计数
+
+// 电量显示LED
+#define LED_BAT 13
+
+// 定义摇杆数值死区参数
+const int deadZone = 15;
+
+typedef struct {
+  byte j1PotX;  // 左摇杆X  (0-255)
+  byte j1PotY;  // 左摇杆Y  (0-255)
+  byte j2PotX;  // 右摇杆X  (0-255)
+  byte j2PotY;  // 右摇杆Y (0-255)
+  bool buttonR1;  //底部右按键 1：true （默认），0：false
+  bool buttonR2;  //底部左按键 1：true （默认），0：false
+  bool j1Button;  //左摇杆按键 1：true （默认），0：false
+  bool j2Button;  //右摇杆按键 1：true （默认），0：false
+  bool buttonLB;  //顶部左按键 1：true （默认），0：false
+  bool buttonRB;  //顶部右按键 1：true （默认），0：false
+  bool tSwitch1;  //拨动开关   1：true （默认），0：false 
+  byte roll;      //用于M5StackCore2或未来增加的陀螺仪功能
+  byte pitch;
+  bool buttonR3;
+} JoystickData;
+
+JoystickData remoteData;
+
+// 蓝灯爆闪
+bool lastbuttonLB = false;
+
+// ESP-NOW接收回调函数
+void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
+  memcpy(&remoteData, incomingData, sizeof(remoteData));
+
+  // 跳跃状态不接受指令
+  if(jump_flag > 0){
+    wrobot.dir = 4; // 不跳跃
+    wrobot.joyx = 0;
+    wrobot.joyy = 0;
+    wrobot.roll = 0;
+    return;
+  }
+
+  // 摇杆输出范围0-255，转换到 -100 到 100
+  // roll 左右摇摆
+  int convertedJ1PotX = (remoteData.j1PotX - 127.5) * (30.0 / 127.5); // 转到 -30 到 30
+  // 上下高度
+  int convertedJ1PotY = static_cast<int>((1 - remoteData.j1PotY / 255.0) * (60 - 0) + 0); // 转到 60 到 0  y轴取反，这样可以使机身下降
+  // 左右旋转
+  int convertedJ2PotX = (remoteData.j2PotX - 127.5) * (100.0 / 127.5); // 转到 -100 到 100
+  // 前后速度
+  int convertedJ2PotY = (remoteData.j2PotY - 127.5) * (100.0 / 127.5) * -1 * 1.4; // 转到 -100 到 100 y轴取反,并且放大洗漱
+  
+  // 取反按钮值，使得默认显示 0 false，按下显示 1 true
+  int convertedbuttonLB = !remoteData.buttonLB; // 顶部左
+  int convertedbuttonRB = !remoteData.buttonRB; // 顶部右
+  // int convertedbuttonR1 = !remoteData.buttonR1; // 底部右
+  // int convertedbuttonR2 = !remoteData.buttonR2; // 底部左
+
+  // 跳跃
+  if (convertedbuttonRB) { // 1=true，0=false
+    wrobot.dir = 5; // 跳跃
+  }else{
+    wrobot.dir = 4; // 不跳跃
+  }
+  // Serial.print(convertedbuttonRB);
+
+  // led蓝灯爆闪
+  if (lastbuttonLB && !convertedbuttonLB) {
+    blinkLed(); // 不能写在loop中，会阻断legloop，小车会倒下
+  }
+  // 更新上次 lastbuttonRB 的值
+  lastbuttonLB = convertedbuttonLB;
+
+  if(abs(convertedJ1PotY) > deadZone) {    // 上下高度 15 ~ 75
+    wrobot.height = convertedJ1PotY;
+  }else{
+    wrobot.height = 0;
+  }
+
+  if(abs(convertedJ1PotX) > deadZone) {    // 左右摇摆 -30 ~ 30
+    wrobot.roll  = convertedJ1PotX;
+  }else{
+    wrobot.roll = 0;
+  }
+  if(abs(convertedJ2PotY) > deadZone) {    // 前后运动 -100 到 100
+    wrobot.joyy = convertedJ2PotY;
+  }else{
+    wrobot.joyy = 0;
+  } 
+  if(abs(convertedJ2PotX) > deadZone) { // 左右运动  -100 到 100
+    wrobot.joyx = convertedJ2PotX; 
+  } else {
+    wrobot.joyx = 0;
+  }
+}
+
+void setup()
+{
+  Serial.begin(115200);   // 通讯串口
+  Serial2.begin(1000000); // 腿部sts舵机
+
+  // Wifi初始化
+  WiFi_SetAP(); 
+  webserver.begin();
+  webserver.on("/", HTTP_GET, basicWebCallback);
+  websocket.begin();
+  websocket.onEvent(webSocketEventCallback);
+
+  // 初始化ESP-NOW
+  WiFi.mode(WIFI_STA); // 打开这行注释就能开启sta模式，同时默认关闭ap热点模式，省电+稳定，而且不会影响espnow通信和整体代码框架
+  if (esp_now_init() != ESP_OK) {
+    Serial.println("Error initializing ESP-NOW");
+    return;
+  }
+  esp_now_register_recv_cb(OnDataRecv);
+
+  // 舵机初始化
+  sms_sts.pSerial = &Serial2;
+  ID[0] = 1;
+  ID[1] = 2;
+  ACC[0] = 30;
+  ACC[1] = 30;
+  Speed[0] = 400;
+  Speed[1] = 400;
+  // Position[0] = 2048;
+  // Position[1] = 2048;
+  Position[0] = SERVO1_MIN;
+  Position[1] = SERVO2_MIN;
+  // 舵机(ID1/ID2)以最高速度V=2400步/秒，加速度A=50(50*100步/秒^2)，运行至各自的Position位置
+  sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
+
+  // 电压检测
+  adc_calibration_init();
+  adc1_config_width(width);
+  adc1_config_channel_atten(channel, atten);
+  esp_adc_cal_characterize(unit, atten, width, 0, &adc_chars);
+
+  // 电量显示LED
+  pinMode(LED_BAT, OUTPUT);
+
+  // 编码器设置
+  I2Cone.begin(19, 18, 400000UL);
+  I2Ctwo.begin(23, 5, 400000UL);
+  sensor1.init(&I2Cone);
+  sensor2.init(&I2Ctwo);
+
+  // mpu6050设置
+  mpu6050.begin();
+  mpu6050.calcGyroOffsets(true);
+
+  // 连接motor对象与编码器对象
+  motor1.linkSensor(&sensor1);
+  motor2.linkSensor(&sensor2);
+
+  // 速度环PID参数
+  motor1.PID_velocity.P = 0.05;
+  motor1.PID_velocity.I = 1;
+  motor1.PID_velocity.D = 0;
+
+  motor2.PID_velocity.P = 0.05;
+  motor2.PID_velocity.I = 1;
+  motor2.PID_velocity.D = 0;
+
+  // 驱动器设置
+  motor1.voltage_sensor_align = 6;
+  motor2.voltage_sensor_align = 6;
+  driver1.voltage_power_supply = 8;
+  driver2.voltage_power_supply = 8;
+  driver1.init();
+  driver2.init();
+
+  // 连接motor对象与驱动器对象
+  motor1.linkDriver(&driver1);
+  motor2.linkDriver(&driver2);
+
+  motor1.torque_controller = TorqueControlType::voltage; // 扭矩控制器类型为 "电压模式"
+  motor2.torque_controller = TorqueControlType::voltage;
+  motor1.controller = MotionControlType::torque; // 运动控制器类型为 "扭矩模式"
+  motor2.controller = MotionControlType::torque;
+    
+
+  // monitor相关设置
+  motor1.useMonitoring(Serial);
+  motor2.useMonitoring(Serial);
+  // 电机初始化
+  motor1.init();
+  motor1.initFOC();
+  motor2.init();
+  motor2.initFOC();
+
+  resetToDefault(); // 重置到默认参数状态
+  delay(500);
+}
+
+void loop()
+{
+  
+  bat_check();        // 电压检测
+  web_loop();         // Web数据更新
+  mpu6050.update();   // IMU数据更新
+  lqr_balance_loop(); // lqr自平衡控制，更新LQR_u
+  yaw_loop();         // yaw轴转向控制，更新YAW_output
+  leg_loop();         // 腿部动作控制，控制舵机SyncWritePosEx
+
+  // 记录上一次的遥控数据数据
+  wrobot.dir_last = wrobot.dir;
+  wrobot.joyx_last = wrobot.joyx;
+  wrobot.joyy_last = wrobot.joyy;
+
+  // 将自平衡计算输出转矩赋给电机
+  // motor.target 负数小车向前转，正数小车向后转，
+  motor1.target = (-0.5) * (LQR_u + YAW_output); // 负数小车向前，正数小车向后，取
+  motor2.target = (-0.5) * (LQR_u - YAW_output);
+
+  // 查看角度信息, 屁股向后倒 -33~-34度,舵机2048的时候-42度，头向前到底 75度，完全垂直88度
+  //  Serial.println(LQR_angle);
+  
+  // 检测跳跃状态
+  // if(jump_flag>0){
+  //   Serial.println(jump_flag);
+  // }
+   
+
+  // 检测小车是否向后马上要倒地了，在中间过程重置所有参数
+  loopCounter++;
+  // loopCounter作用：在刚开机的800次loop循环，不能进入resetToDefault()函数
+  // resetToDefaultCounter作用：防止小车倒地后某个角度，陷入循环，motor.target一直等于0，就躺地上不动了
+  if ((LQR_angle < -35.0f || LQR_angle > 70.0f )&& loopCounter > 800 && resetToDefaultCounter < 130) {
+      resetToDefault();
+      resetToDefaultCounter++;
+      motor1.target = 0;
+      motor2.target = 0;
+      // Serial.println("resetToDefault");
+  }
+
+  if( abs(LQR_angle) < 25.0f){
+    resetToDefaultCounter = 0;
+  }
+
+  // target目标电压 0~8V, 0为不转, 8为最大转速
+  // Serial.print("LQR_angle:");
+  // Serial.print(LQR_angle);
+  // Serial.print(",motor1.target:");
+  // Serial.print(motor1.target);
+  // Serial.print(",motor2.target:");
+  // Serial.println(motor2.target);
+
+  // 迭代计算FOC相电压
+  motor1.loopFOC();
+  motor2.loopFOC();
+
+  // 设置轮部电机输出
+  motor1.move();
+  motor2.move();
+
+  jump_loop(); // 跳跃控制
+
+} // loop()
+
+// lqr自平衡控制
+void lqr_balance_loop()
+{
+
+  // LQR_distance,LQR_speed,LQR_angle,LQR_gyro 通过编码器或者mpu实际测得
+  // angle_zeropoint，distance_zeropoint 角度零点和位置零点（在理想平衡状态下俯仰角度和位移目标值），用于和实际值做差，得到控制
+  // lqr 控制参数： angle_control , gyro_control , distance_control , speed_control
+  
+  // LQR平衡算式，实际使用中为便于调参，讲算式分解为4个P控制，采用PIDController方法在commander中实时调试
+  // LQR_u = LQR_k1*(LQR_angle - angle_zeropoint) 
+        // + LQR_k2*LQR_gyro 
+        // + LQR_k3*(LQR_distance - distance_zeropoint) 
+        // + LQR_k4*LQR_speed;
+
+  // 给负值是因为按照当前的电机接线，正转矩驱动小车电机向后转
+
+  /*
+  LQR_u 越大，电机输出的转矩越大，电机向后转，默认0-8V
+  LQR_u = angle_control + gyro_control + distance_control + speed_control
+
+  编码器反馈shaft_angle角度，shaft_velocity速度
+  不操作前后移动，譬如刚开机，车轮不动的，shaft_angle和shaft_velocity 都是零
+  所以，不操作前后移动，LQR_distance 和 LQR_speed趋近于0，只有角度控制和角速度控制
+
+   angle_control = pid_angle(LQR_angle - angle_zeropoint) //LQR_u与实际LQR_angle正相关
+                 = pid_angle( mpu6050.getAngleY() - angle_zeropoint) //LQR_u与实际pitch正相关
+   gyro_control = pid_gyro(LQR_gyro) //LQR_u与实际LQR_gyro正相关
+                = pid_gyro( mpu6050.getGyroY() ) //LQR_u与实际pitch 角速度 正相关
+    
+    所以这里LQR_distance和LQR_speed的计算乘以-0.5,不能单看负号
+   distance_control = pid_distance(LQR_distance - distance_zeropoint) //LQR_u与电LQR_distance正相关 
+                    = pid_distance( (-0.5)*(motor1.shaft_angle + motor2.shaft_angle) - distance_zeropoint)  
+                    //在平衡车中，通常设定电机向后转（使车前进）为负，所以LQR_u与电机实际shaft_angle角度正相关，和负号没关系
+   
+    speed_control = pid_speed(LQR_speed - 0.1 * lpf_joyy(wrobot.joyy)) // 实际速度-目标速度
+                  //LQR_speed代表实际速度，LQR_u与实际LQR_speed速度正相关
+                  = pid_speed( (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity) - 0.1 * lpf_joyy(wrobot.joyy)) 
+                  //LQR_u与实际shaft_velocity速度正相关
+  */
+
+  // 在平衡车中，通常设定电机向后转（使车前进）为负，所以这里LQR_distance和LQR_speed的计算乘以-0.5
+  LQR_distance = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); // 两个电机的旋转角度（shaft_angle）（是位置量）的平均值
+  LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity); // 两个电机角速度（shaft_velocity）的平均值，并取反
+  LQR_angle = (float)mpu6050.getAngleY(); // mpu6050 pitch 角度，向前倒正数，向后倒负数
+  LQR_gyro = (float)mpu6050.getGyroY(); // pitch Y轴角速度
+
+  // 计算pitch的角度和角速度的pid控制值
+  // 实际pitch角度与零点的差值，目标值是零
+  // 计算角度误差，并通过 PID 控制器得到角度控制量
+  angle_control = pid_angle(LQR_angle - angle_zeropoint); 
+  // 实际pitch 角速度与目标值的差值，目标值是零
+  // 通过 PID 控制器对角速度进行处理，得到角速度控制量
+  gyro_control = pid_gyro(LQR_gyro);
+
+  // 运动细节优化处理
+  if (wrobot.joyy != 0) // 有前后方向运动指令时的处理
+  {
+    distance_zeropoint = LQR_distance; // 位移零点重置
+    pid_lqr_u.error_prev = 0;          // 输出积分清零
+  }
+  // joyy=0，没有输出前后运动，但是小车可能被推动，运动指令复零时的原地停车处理
+  if ((wrobot.joyx_last != 0 && wrobot.joyx == 0) || (wrobot.joyy_last != 0 && wrobot.joyy == 0)) 
+  {
+    wrobot_move_stop_flag = 1;
+  }
+  if ((wrobot_move_stop_flag == 1) && (abs(LQR_speed) < 0.5))
+  {
+    distance_zeropoint = LQR_distance; // 位移零点重置
+    wrobot_move_stop_flag = 0;
+  }
+
+  if (abs(LQR_speed) > 15) // 被快速推动时的原地停车处理
+  {
+    distance_zeropoint = LQR_distance; // 位移零点重置
+  }
+
+  // 计算pitch的角度和角速度的pid控制值
+  // 电机转轴角度平均数(上面计算)-位移零点=实际位移与零点的差值, 目标值是零
+  // 计算位移误差，并通过位移 PID 控制器得到位移控制量
+  distance_control = pid_distance(LQR_distance - distance_zeropoint); 
+  // 电机转速平均数(上面计算)-速度目标值=实际转速与目标值的差值, 目标值是零
+  // 计算速度误差（实际速度减去经过低通滤波和缩放后的遥控器期望速度），并通过速度 PID 控制器得到速度控制量
+  speed_control = pid_speed(LQR_speed - 0.1 * lpf_joyy(wrobot.joyy));
+
+  // 轮部离地检测, 连续记录两次的轮部转速
+  robot_speed_last = robot_speed; // 记录连续两次的轮部转速
+  robot_speed = LQR_speed;
+  // 若轮部角速度、角加速度"过大"（ > 10或> 50）,或处于跳跃后的恢复时期（jump_flag != 0），认为出现轮部离地现象，需要特殊处理
+  // 轮部离地情况下 LQR_u计算：2个参数，角度控制量+角速度控制量，对轮位移(distance)和速度(speed)控制分量不输出；反之，正常状态下完整输出平衡转矩
+  if (abs(robot_speed - robot_speed_last) > 10 || abs(robot_speed) > 50 || (jump_flag != 0)) 
+  {
+    distance_zeropoint = LQR_distance;    // 位移零点重置
+    LQR_u = angle_control + gyro_control;  // 位移和速度控制分量被忽略
+    pid_lqr_u.error_prev = 0;             // 输出积分清零
+  }
+  else // 当轮部未离地时，LQR_u：4个参数
+  {
+    // 当轮部未离地时，LQR_u 包含:角度控制量、角速度控制量、位移控制量和速度控制量，以实现完整的自平衡控制
+    LQR_u = angle_control + gyro_control + distance_control + speed_control;
+    // 计算得到LQR_u在loop()函数中在几个子loop执行完，被motor1和motor2执行
+    
+  }
+
+  // 小车没有控制的时候自稳定状态
+  // 控制量lqr_u<5，控制量小， 遥控器无信号输入joyy=0，轮部位移控制正常介入distance_control<4，不处于跳跃后的恢复时期jump_flag=0
+  if (abs(LQR_u) < 5 && wrobot.joyy == 0 && abs(distance_control) < 4 && (jump_flag == 0))
+  {
+
+    LQR_u = pid_lqr_u(LQR_u); // 小转矩非线性补偿
+    // Serial.println(LQR_u);
+
+    // lpf_zeropoint 是一个低通滤波器，对 distance_control 进行滤波处理，减少高频噪声的影响。
+    // 然后，将滤波后的 distance_control 作为输入，通过 pid_zeropoint 控制器进行进一步处理。
+    // angle_zeropoint -= ...：将计算得到的调整量从 angle_zeropoint 中减去，实现重心自适应调整。
+    // angle_zeropoint 是预设的角度零点，调整它可以让小车更好地适应不同的重心位置。
+    angle_zeropoint -= pid_zeropoint(lpf_zeropoint(distance_control)); // 重心自适应
+  }
+  else
+  {
+    pid_lqr_u.error_prev = 0; // 输出积分清零
+  }
+
+  // 平衡控制参数自适应
+  if (wrobot.height < 50)
+  {
+    pid_speed.P = 0.7;
+  }
+  else if (wrobot.height < 64)
+  {
+    pid_speed.P = 0.6;
+  }
+  else
+  {
+    pid_speed.P = 0.5;
+  }
+
+}
+
+// 腿部动作控制（舵机）
+void leg_loop()
+{
+
+  // 倒地时强制腿部收缩
+  if (LQR_angle < -30.0f ||  LQR_angle > 45.0f) 
+  {
+      Position[0] = SERVO1_MIN;
+      Position[1] = SERVO2_MIN;
+      sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
+      return; // 跳过后续控制
+  }
+
+  if (jump_flag == 0) // 不处于跳跃状态
+  {
+    // 舵机加速度
+    ACC[0] = 8;
+    ACC[1] = 8;
+
+    // 舵机速度
+    Speed[0] = 300;
+    Speed[1] = 300;
+    float roll_angle = (float)mpu6050.getAngleX() + 2.0;
+    leg_position_add = pid_roll_angle(lpf_roll(roll_angle));
+
+    int roll_offset = wrobot.roll;
+    int left_height, right_height;
+
+    // Serial.print("roll_offset: ");
+    // Serial.println(roll_offset);
+
+    // 如果控制了 roll 角度，则调整左右脚的高度
+    // 计算三角函数值
+    float angleRad = abs(roll_offset) * (pi / 180.0); // 将角度转换为弧度
+    float heightAdjust = 30 * sin(angleRad); // 小车宽度的一半是30毫米，抬高的高度是30毫米*sin(弧度)
+    float RollOffset = 18; // 所以水平高度需要加上调整高度，否则向下倾斜无法向下了
+    if (roll_offset > 10) {
+      // 正数，左脚高度 60，右脚高度 40
+      // 因为车身高度最小32，所以水平高度需要加上调整高度15，否则线下倾斜无法向下了
+      left_height = wrobot.height + RollOffset + heightAdjust;
+      right_height = wrobot.height + RollOffset - heightAdjust;
+      leg_position_add = 0; // 不使用roll角度 pid计算
+    } else if (roll_offset < -10) {
+      // 负数，左脚高度 40，右脚高度 60
+      left_height = wrobot.height + RollOffset - heightAdjust;
+      right_height = wrobot.height + RollOffset + heightAdjust;
+      leg_position_add = 0;
+    } else { // 未控制 roll 角度
+      // roll 为 0，左右恢复到同一高度
+      left_height = wrobot.height;
+      right_height = wrobot.height;
+      // 没有人为控制roll角度，leg_position_add根据mpu6050 roll值 进行pid计算得到，以保持小车机身平稳
+      // 但如果小车倒地，则不需要keep roll角度,否则小车倒在地上，1个脚长，1个脚短，很难看
+      if (abs(LQR_angle) > 25.0f) 
+      {
+        leg_position_add = 0;
+      }
+    }
+
+    // 根据新的高度计算舵机位置
+    // id 1 [0] 正面看右手，id 2 正面看左手 [1]
+    // 舵机id 1：1900（机身最矮）  2400（机身最高）2400-1900=500
+    // 舵机id 2：2300（机身最矮）  1800（机身最高）2300-1800=500
+    // 2048是舵机中间位置，12是微调偏移量，8.4是系数，32是默认高度，最高80，最低32
+    // const int LEG_HEIGHT_BASE = 32; // 这值越小，机身越高，实测范围 0（最高）-52（最低）
+    Position[0] = 2048 + 12 + 8.4 * (left_height - LEG_HEIGHT_BASE) - leg_position_add;
+    Position[1] = 2048 - 12 - 8.4 * (right_height - LEG_HEIGHT_BASE) - leg_position_add;
+
+    // 限制舵机位置范围
+    if (Position[0] < SERVO1_MIN) // 舵机1最低
+      Position[0] = SERVO1_MIN;
+    else if (Position[0] > SERVO1_MAX) // 舵机1最高
+      Position[0] = SERVO1_MAX;
+
+    if (Position[1] > SERVO2_MIN) // 舵机2最低（和舵机1方向是反的）
+      Position[1] = SERVO2_MIN;
+    else if (Position[1] < SERVO2_MAX) // 舵机2最高
+      Position[1] = SERVO2_MAX;
+    
+    sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
+  }
+}
+
+// 跳跃控制
+void jump_loop()
+{ // dir: 4 = STOP, 5 = jump 上次动作是跳跃5，本次是停止4，才开始跳，因为可能连续发送了很多5跳跃指令，只执行最后一次
+  // 跳跃，腿部以最快速度伸到最长
+  if ((wrobot.dir == 5) && (jump_flag == 0))
+  { 
+
+    // 跳跃开始
+    ACC[0] = 0;
+    ACC[1] = 0;
+    Speed[0] = 0;
+    Speed[1] = 0;
+    Position[0] = SERVO1_MAX; 
+    Position[1] = SERVO2_MAX;
+    sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
+
+    jump_flag = 1;
+
+  }
+  if (jump_flag > 0) // 跳跃中，腿部缩起来，等待30帧，对应上面设置 jump_flag = 1;
+  {
+    jump_flag++;
+
+    if ((jump_flag > 30) && (jump_flag < 35))
+    {
+      ACC[0] = 0;
+      ACC[1] = 0;
+      Speed[0] = 0;
+      Speed[1] = 0;
+      // Position[0] = 2048 + 12 + 8.4 * (35 - 32); // 降低腿部高度，即车重心
+      // Position[1] = 2048 - 12 - 8.4 * (35 - 32); 
+      Position[0] = SERVO1_MIN + 150; // 收缩腿
+      Position[1] = SERVO2_MIN - 150;
+      sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
+
+      jump_flag = 40;
+    }
+    if (jump_flag > 200)
+    {
+      jump_flag = 0; // 跳跃过程结束
+    }
+  }
+}
+
+// yaw轴转向控制 （转向控制）
+void yaw_loop()
+{
+  // YAW_output = 0.03*(YAW_Kp*YAW_angle_total + YAW_Kd*YAW_gyro);
+  yaw_angle_addup();
+
+  YAW_angle_total += wrobot.joyx * 0.01; // 0.002
+  float yaw_angle_control = pid_yaw_angle(YAW_angle_total);
+  float yaw_gyro_control = pid_yaw_gyro(YAW_gyro);
+  //  yaw 轴的角度和角速度数据进行了 PID 控制，得到 yaw_angle_control 和 yaw_gyro_control。
+  // 然后，将这两个控制量相加，得到最终的 YAW_output。
+  YAW_output = yaw_angle_control + yaw_gyro_control;
+}
+
+// Web数据更新
+void web_loop()
+{
+  webserver.handleClient();
+  websocket.loop();
+  rp.spinOnce(); // 更新web端回传的控制信息
+}
+
+// yaw轴角度累加函数
+void yaw_angle_addup()
+{
+  YAW_angle = (float)mpu6050.getAngleZ();
+  YAW_gyro = (float)mpu6050.getGyroZ();
+
+  if (YAW_angle_zero_point == (-10))
+  {
+    YAW_angle_zero_point = YAW_angle;
+  }
+
+  float yaw_angle_1, yaw_angle_2, yaw_addup_angle;
+  if (YAW_angle > YAW_angle_last)
+  {
+    yaw_angle_1 = YAW_angle - YAW_angle_last;
+    yaw_angle_2 = YAW_angle - YAW_angle_last - 2 * PI;
+  }
+  else
+  {
+    yaw_angle_1 = YAW_angle - YAW_angle_last;
+    yaw_angle_2 = YAW_angle - YAW_angle_last + 2 * PI;
+  }
+
+  if (abs(yaw_angle_1) > abs(yaw_angle_2))
+  {
+    yaw_addup_angle = yaw_angle_2;
+  }
+  else
+  {
+    yaw_addup_angle = yaw_angle_1;
+  }
+
+  YAW_angle_total = YAW_angle_total + yaw_addup_angle;
+  YAW_angle_last = YAW_angle;
+}
+
+void basicWebCallback(void)
+{
+  webserver.send(300, "text/html", basic_web);
+}
+
+void webSocketEventCallback(uint8_t num, WStype_t type, uint8_t *payload, size_t length)
+{
+  if (type == WStype_TEXT)
+  {
+    String payload_str = String((char *)payload);
+    StaticJsonDocument<300> doc;
+    DeserializationError error = deserializeJson(doc, payload_str);
+
+    String mode_str = doc["mode"];
+    if (mode_str == "basic")
+    {
+      rp.parseBasic(doc);
+    }
+  }
+}
+
+// 电压检测初始化
+void adc_calibration_init()
+{
+  if (esp_adc_cal_check_efuse(ESP_ADC_CAL_VAL_EFUSE_TP) == ESP_OK)
+  {
+    printf("eFuse Two Point: Supported\n");
+  }
+  else
+  {
+    printf("eFuse Two Point: NOT supported\n");
+  }
+  // Check Vref is burned into eFuse
+  if (esp_adc_cal_check_efuse(ESP_ADC_CAL_VAL_EFUSE_VREF) == ESP_OK)
+  {
+    printf("eFuse Vref: Supported\n");
+  }
+  else
+  {
+    printf("eFuse Vref: NOT supported\n");
+  }
+}
+
+// 电压检测
+void bat_check()
+{
+  if (bat_check_num > 1000)
+  {
+    // 电压读取
+    uint32_t sum = 0;
+    sum = analogRead(BAT_PIN);
+    uint32_t voltage = esp_adc_cal_raw_to_voltage(sum, &adc_chars);
+    double battery = (voltage * 3.97) / 1000.0;
+
+    // Serial.println(battery);
+    // 电量显示
+    if (battery > 7.8)
+      digitalWrite(LED_BAT, HIGH);
+    else
+      digitalWrite(LED_BAT, LOW);
+
+    bat_check_num = 0;
+  }
+  else
+    bat_check_num++;
+}
+
+// LED闪烁函数(不要用for，会阻塞其他程序)
+void blinkLed() {
+  digitalWrite(LED_BAT, HIGH);
+  delay(200);
+  digitalWrite(LED_BAT, LOW);
+  delay(200);
+  digitalWrite(LED_BAT, HIGH);
+  delay(200);
+  digitalWrite(LED_BAT, LOW);
+  delay(200);
+}
+
+// 重置到默认参数状态，用于自平衡起立
+void resetToDefault() {
+    // LQR自平衡控制器参数
+    LQR_angle = 0;
+    LQR_gyro = 0;
+    angle_control = 0;
+    gyro_control = 0;
+    angle_zeropoint = 5;
+
+    LQR_speed = 0;
+    LQR_distance = 0;
+    speed_control = 0;
+    distance_control = 0;
+    distance_zeropoint = 1;
+    LQR_u = 0;
+    
+    // YAW轴控制数据
+    YAW_gyro = 0;
+    YAW_angle = 0;
+    YAW_angle_last = 0;
+    YAW_angle_total = 0;
+    YAW_angle_zero_point = -10;
+    YAW_output = 0;
+
+    // 逻辑处理标志位
+    robot_speed = 0;
+    robot_speed_last = 0;
+    wrobot_move_stop_flag = 0;
+    jump_flag = 0;
+    leg_position_add = 0;
+
+    pid_lqr_u.error_prev = 0;
+
+    
+
+}

Shibo-main-TOMMY/test/main 障碍.cpp

@@ -0,0 +1,792 @@
+// -----------------------------------------------------------------------------
+// Copyright (c) 2024 Mu Shibo
+// Modify by Tommy 2025.8.11
+// -----------------------------------------------------------------------------
+
+#include <esp_now.h>
+#include <WiFi.h>
+#include "main.h"
+
+/************实例定义*************/
+
+// 电机实例
+BLDCMotor motor1 = BLDCMotor(7);
+BLDCMotor motor2 = BLDCMotor(7);
+BLDCDriver3PWM driver1 = BLDCDriver3PWM(32, 33, 25, 22);
+BLDCDriver3PWM driver2 = BLDCDriver3PWM(26, 27, 14, 12);
+
+// 编码器实例
+TwoWire I2Cone = TwoWire(0);
+TwoWire I2Ctwo = TwoWire(1);
+MagneticSensorI2C sensor1 = MagneticSensorI2C(AS5600_I2C);
+MagneticSensorI2C sensor2 = MagneticSensorI2C(AS5600_I2C);
+
+// PID控制器实例
+PIDController pid_angle(1, 0, 0, 100000, 8); // pitch角度PID使其趋于零点
+PIDController pid_gyro(0.06, 0, 0, 100000, 8); // 角速度PID使其趋于零点
+PIDController pid_distance(0.5, 0, 0, 100000, 8);// 位移PID使其趋于零点
+PIDController pid_speed(0.7, 0, 0, 100000, 8);// 速度PID使其趋于目标速度
+PIDController pid_yaw_angle(1.0, 0, 0, 100000, 8);
+PIDController pid_yaw_gyro(0.04, 0, 0, 100000, 8);
+PIDController pid_lqr_u(1, 15, 0, 100000, 8);
+PIDController pid_zeropoint(0.002, 0, 0, 100000, 4);
+PIDController pid_roll_angle(8, 0, 0, 100000, 450);
+
+// 低通滤波器实例
+LowPassFilter lpf_joyy(0.2);
+LowPassFilter lpf_zeropoint(0.1);
+LowPassFilter lpf_roll(0.3);
+
+// WebServer实例
+WebServer webserver;                               // server服务器
+WebSocketsServer websocket = WebSocketsServer(81); // 定义一个webSocket服务器来处理客户发送的消息
+RobotProtocol rp(20);
+
+// STS舵机实例
+SMS_STS sms_sts;
+
+// MPU6050实例
+MPU6050 mpu6050(I2Ctwo);
+
+/************参数定义*************/
+#define pi 3.1415927
+
+// 舵机位置限制参数
+#define SERVO1_MIN 1900 // 舵机1最低 从小车正面看右手
+#define SERVO1_MAX 2400 // 舵机1最高
+#define SERVO2_MIN 2300 // 舵机2最低 从小车正面看左手(注意大小值与舵机1是相反的)
+#define SERVO2_MAX 1800 // 舵机2最高
+
+// LQR自平衡控制器参数
+float LQR_angle = 0;
+float LQR_gyro = 0;
+float LQR_speed = 0;
+float LQR_distance = 0;
+float angle_control = 0;
+float gyro_control = 0;
+float speed_control = 0;
+float distance_control = 0;
+float LQR_u = 0;
+float angle_zeropoint = 5;  // 俯仰角度，向前到正，向后倒负
+float distance_zeropoint = 1; // 轮部位移零点偏置
+
+// YAW轴控制数据
+float YAW_gyro = 0;
+float YAW_angle = 0;
+float YAW_angle_last = 0;
+float YAW_angle_total = 0;
+float YAW_angle_zero_point = -10;
+float YAW_output = 0;
+
+// 腿部舵机控制数据
+byte ID[2];
+s16 Position[2];
+u16 Speed[2];
+byte ACC[2];
+
+// 逻辑处理标志位
+float robot_speed = 0;         // 记录当前轮部转速
+float robot_speed_last = 0;    // 记录上一时刻的轮部转速
+int wrobot_move_stop_flag = 0; // 记录摇杆停止的标识
+int jump_flag = 0;             // 跳跃时段标识
+float leg_position_add = 0;    // roll轴平衡控制量
+
+// 电压检测
+uint16_t bat_check_num = 0;
+int BAT_PIN = 35; // select the input pin for the ADC
+static esp_adc_cal_characteristics_t adc_chars;
+static const adc1_channel_t channel = ADC1_CHANNEL_7;
+static const adc_bits_width_t width = ADC_WIDTH_BIT_12;
+static const adc_atten_t atten = ADC_ATTEN_DB_11;
+static const adc_unit_t unit = ADC_UNIT_1;
+
+// 电量显示LED
+#define LED_BAT 13
+
+// LED闪烁控制变量
+bool blinking = false;
+unsigned long lastBlinkTime = 0;
+int blinkCount = 0;
+const int BLINK_INTERVAL = 200; // 闪烁间隔(ms)
+const int BLINK_TIMES = 5;      // 闪烁次数
+// 在参数定义部分添加障碍物检测相关变量
+bool obstacleDetected = false;      // 障碍物检测状态
+unsigned long obstacleStartTime = 0; // 障碍物开始时间
+const unsigned long OBSTACLE_DELAY = 500; // 判定为障碍物的延迟时间(毫秒)
+// 在参数定义部分新增扭矩补偿相关变量
+const unsigned long BOOST_DURATION = 1500; // 最大补偿时长（3秒，避免持续高负载）
+unsigned long boostStartTime = 0;      // 补偿开始时间
+
+
+// 摇杆数据死区参数
+const int deadZone = 15;
+typedef struct {
+  byte j1PotX;  // 左摇杆X  (0-255)
+  byte j1PotY;  // 左摇杆Y  (0-255)
+  byte j2PotX;  // 右摇杆X  (0-255)
+  byte j2PotY;  // 右摇杆Y (0-255)
+  bool buttonR1;  //底部右按键 1：true （默认），0：false
+  bool buttonR2;  //底部左按键 1：true （默认），0：false
+  bool j1Button;  //左摇杆按键 1：true （默认），0：false
+  bool j2Button;  //右摇杆按键 1：true （默认），0：false
+  bool buttonLB;  //顶部左按键 1：true （默认），0：false
+  bool buttonRB;  //顶部右按键 1：true （默认），0：false
+  bool tSwitch1;  //拨动开关   1：true （默认），0：false 
+  byte roll;      //用于M5StackCore2或未来增加的陀螺仪功能
+  byte pitch;
+  bool buttonR3;
+} JoystickData;
+
+JoystickData remoteData;
+
+// 蓝灯爆闪
+bool lastbuttonLB = false;
+
+// ESP-NOW接收回调函数
+void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
+  memcpy(&remoteData, incomingData, sizeof(remoteData));
+
+  // 摇杆输出范围0-255，转换到 -100 到 100
+  // roll 左右摇摆
+  int convertedJ1PotX = (remoteData.j1PotX - 127.5) * (30.0 / 127.5); // 转到 -30 到 30
+  // 上下高度
+  int convertedJ1PotY = static_cast<int>((1 - remoteData.j1PotY / 255.0) * (60 - 0) + 0); // 转到 60 到 0  y轴取反，这样可以使机身下降
+  // 左右旋转
+  int convertedJ2PotX = (remoteData.j2PotX - 127.5) * (100.0 / 127.5); // 转到 -100 到 100
+  // 速度
+  int convertedJ2PotY = (remoteData.j2PotY - 127.5) * (100.0 / 127.5) * -1 * 1.3; // 转到 -100 到 100 y轴取反,并且放大洗漱
+  
+  // 取反按钮值，使得默认显示 0 false，按下显示 1 true
+  int convertedbuttonLB = !remoteData.buttonLB; // 顶部左
+  int convertedbuttonRB = !remoteData.buttonRB; // 顶部右
+
+  // 跳跃
+  if (convertedbuttonRB) { // 1=true，0=false
+    wrobot.dir = 5; // 跳跃
+  }else{
+    wrobot.dir = 4; // 不跳跃
+  }
+
+  // led蓝灯爆闪
+  if (lastbuttonLB && !convertedbuttonLB) {
+    blinkLed(); // 不能写在loop中，会阻断legloop，小车会倒下
+  }
+  // 更新上次 lastbuttonRB 的值
+  lastbuttonLB = convertedbuttonLB;
+
+  if(abs(convertedJ1PotY) > deadZone) {    // 上下高度 15 ~ 75
+    wrobot.height = convertedJ1PotY;
+  }else{
+    wrobot.height = 0;
+  }
+
+  if(abs(convertedJ1PotX) > deadZone) {    // 左右摇摆 -30 ~ 30
+    wrobot.roll  = convertedJ1PotX;
+  }else{
+    wrobot.roll = 0;
+  }
+  if(abs(convertedJ2PotY) > deadZone) {    // 前后运动 -100 到 100
+    wrobot.joyy = convertedJ2PotY;
+  }else{
+    wrobot.joyy = 0;
+  } 
+  if(abs(convertedJ2PotX) > deadZone) { // 左右运动  -100 到 100
+    wrobot.joyx = convertedJ2PotX; 
+  } else {
+    wrobot.joyx = 0;
+  }
+}
+
+void setup()
+{
+  Serial.begin(115200);   // 通讯串口
+  Serial2.begin(1000000); // 腿部sts舵机
+
+  // Wifi初始化
+  WiFi_SetAP(); 
+  webserver.begin();
+  webserver.on("/", HTTP_GET, basicWebCallback);
+  websocket.begin();
+  websocket.onEvent(webSocketEventCallback);
+
+  // 初始化ESP-NOW
+  WiFi.mode(WIFI_STA); // 打开这行注释就能开启sta模式，同时默认关闭ap热点模式，省电+稳定，而且不会影响espnow通信和整体代码框架
+  if (esp_now_init() != ESP_OK) {
+    Serial.println("Error initializing ESP-NOW");
+    return;
+  }
+  esp_now_register_recv_cb(OnDataRecv);
+
+  // 舵机初始化
+  sms_sts.pSerial = &Serial2;
+  ID[0] = 1;
+  ID[1] = 2;
+  ACC[0] = 30;
+  ACC[1] = 30;
+  Speed[0] = 400;
+  Speed[1] = 400;
+  Position[0] = 2048;
+  Position[1] = 2048;
+  // 舵机(ID1/ID2)以最高速度V=2400步/秒，加速度A=50(50*100步/秒^2)，运行至各自的Position位置
+  sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
+
+  // 电压检测
+  adc_calibration_init();
+  adc1_config_width(width);
+  adc1_config_channel_atten(channel, atten);
+  esp_adc_cal_characterize(unit, atten, width, 0, &adc_chars);
+
+  // 电量显示LED
+  pinMode(LED_BAT, OUTPUT);
+
+  // 编码器设置
+  I2Cone.begin(19, 18, 400000UL);
+  I2Ctwo.begin(23, 5, 400000UL);
+  sensor1.init(&I2Cone);
+  sensor2.init(&I2Ctwo);
+
+  // mpu6050设置
+  mpu6050.begin();
+  mpu6050.calcGyroOffsets(true);
+
+  // 连接motor对象与编码器对象
+  motor1.linkSensor(&sensor1);
+  motor2.linkSensor(&sensor2);
+
+  // 速度环PID参数
+  motor1.PID_velocity.P = 0.05;
+  motor1.PID_velocity.I = 1;
+  motor1.PID_velocity.D = 0;
+
+  motor2.PID_velocity.P = 0.05;
+  motor2.PID_velocity.I = 1;
+  motor2.PID_velocity.D = 0;
+
+  // 驱动器设置
+  motor1.voltage_sensor_align = 6;
+  motor2.voltage_sensor_align = 6;
+  driver1.voltage_power_supply = 8;
+  driver2.voltage_power_supply = 8;
+  driver1.init();
+  driver2.init();
+
+  // 连接motor对象与驱动器对象
+  motor1.linkDriver(&driver1);
+  motor2.linkDriver(&driver2);
+
+  motor1.torque_controller = TorqueControlType::voltage;// 扭矩控制类型 :电压作为扭矩控制的方式
+  motor2.torque_controller = TorqueControlType::voltage;
+  motor1.controller = MotionControlType::torque; // 运动控制类型:扭矩控制模式
+  motor2.controller = MotionControlType::torque; // 扭矩控制模式
+
+  // monitor相关设置
+  motor1.useMonitoring(Serial);
+  motor2.useMonitoring(Serial);
+  // 电机初始化
+  motor1.init();
+  motor1.initFOC();
+  motor2.init();
+  motor2.initFOC();
+
+  delay(500);
+}
+
+void loop()
+{
+  
+  bat_check();        // 电压检测
+  web_loop();         // Web数据更新
+  mpu6050.update();   // IMU数据更新
+  lqr_balance_loop(); // lqr自平衡控制，更新LQR_u
+  yaw_loop();         // yaw轴转向控制，更新YAW_output
+  leg_loop();         // 腿部动作控制，控制舵机SyncWritePosEx，包括判断jump_loop
+  detectObstacle();  // 障碍物检测
+  
+
+  // 记录上一次的遥控数据数据
+  wrobot.dir_last = wrobot.dir;
+  wrobot.joyx_last = wrobot.joyx;
+  wrobot.joyy_last = wrobot.joyy;
+
+  // 将自平衡计算输出转矩赋给电机
+  motor1.target = (-0.5) * (LQR_u + YAW_output);
+  motor2.target = (-0.5) * (LQR_u - YAW_output);
+  
+  // 查看角度信息, 屁股向后倒 -36度，头向前到底 45度
+  //  Serial.println(LQR_angle)
+
+  // 向后倒地 -36度，向前倒地 45度，超出这个区间，或者跳跃中，马达就不要转动了
+  if(LQR_angle < -40.0f ||  LQR_angle > 55.0f ) {
+    motor1.target = 0;
+    motor2.target = 0;
+  }
+
+  // 计算基础扭矩（原有逻辑）
+  float baseTorque1 = (-0.5) * (LQR_u + YAW_output);
+  float baseTorque2 = (-0.5) * (LQR_u - YAW_output);
+  
+  // 障碍物扭矩补偿逻辑
+  if (obstacleDetected) {
+    // 首次进入障碍状态时记录时间
+    if (boostStartTime == 0) {
+      boostStartTime = millis();
+    }
+    
+    // 补偿时长未超时且机器人未严重倾斜时，应用扭矩补偿
+    if (millis() - boostStartTime < BOOST_DURATION) {
+      // blinkLed();
+      // 叠加补偿扭矩（仅正向，因为是前进障碍）
+      motor1.target = motor1.voltage_limit;
+      motor2.target = motor2.voltage_limit;
+    } else {
+      // 超时或姿态异常时使用基础扭矩
+      motor1.target = baseTorque1;
+      motor2.target = baseTorque2;
+    }
+  } else {
+    // 无障碍物时重置补偿计时器并使用基础扭矩
+    boostStartTime = 0;
+    motor1.target = baseTorque1;
+    motor2.target = baseTorque2;
+  }
+
+  // 迭代计算FOC相电压
+  motor1.loopFOC();
+  motor2.loopFOC();
+
+  // 设置轮部电机输出
+  motor1.move();
+  motor2.move();
+} // loop()
+
+
+// lqr自平衡控制
+void lqr_balance_loop()
+{
+
+  // LQR_distance,LQR_speed,LQR_angle,LQR_gyro 通过编码器或者mpu实际测得
+  // angle_zeropoint，distance_zeropoint 角度零点和位置零点（在理想平衡状态下俯仰角度和位移目标值），用于和实际值做差，得到控制
+  // angle_control , gyro_control , distance_control , speed_control : lqr 控制参数
+  
+  // LQR平衡算式，实际使用中为便于调参，讲算式分解为4个P控制，采用PIDController方法在commander中实时调试
+  // LQR_u = LQR_k1*(LQR_angle - angle_zeropoint) 
+        // + LQR_k2*LQR_gyro 
+        // + LQR_k3*(LQR_distance - distance_zeropoint) 
+        // + LQR_k4*LQR_speed;
+
+  // 给负值是因为按照当前的电机接线，正转矩会向后转
+  // 编码器反馈shaft_angle角度，shaft_velocity速度
+  LQR_distance = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); // 两个电机的旋转角度（shaft_angle）（是位置量）的平均值
+  LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity); // 两个电机角速度（shaft_velocity）的平均值，并取反
+  LQR_angle = (float)mpu6050.getAngleY(); // mpu6050 pitch 角度，向前倒正数，向后倒负数
+  LQR_gyro = (float)mpu6050.getGyroY(); // pitch Y轴角速度
+
+  // 计算pitch的角度和角速度的pid控制值
+  // 实际pitch角度与零点的差值，目标值是零
+  // 计算角度误差，并通过 PID 控制器得到角度控制量
+  angle_control = pid_angle(LQR_angle - angle_zeropoint); 
+  // 实际pitch 角速度与目标值的差值，目标值是零
+  // 通过 PID 控制器对角速度进行处理，得到角速度控制量
+  gyro_control = pid_gyro(LQR_gyro);
+
+  // 运动细节优化处理
+  if (wrobot.joyy != 0) // 有前后方向运动指令时的处理
+  {
+    distance_zeropoint = LQR_distance; // 位移零点重置
+    pid_lqr_u.error_prev = 0;          // 输出积分清零
+  }
+  // joyy=0，没有输出前后运动，但是小车可能被推动，运动指令复零时的原地停车处理
+  if ((wrobot.joyx_last != 0 && wrobot.joyx == 0) || (wrobot.joyy_last != 0 && wrobot.joyy == 0)) 
+  {
+    wrobot_move_stop_flag = 1;
+  }
+  if ((wrobot_move_stop_flag == 1) && (abs(LQR_speed) < 0.5))
+  {
+    distance_zeropoint = LQR_distance; // 位移零点重置
+    wrobot_move_stop_flag = 0;
+  }
+
+  if (abs(LQR_speed) > 15) // 被快速推动时的原地停车处理
+  {
+    distance_zeropoint = LQR_distance; // 位移零点重置
+  }
+
+  // 计算pitch的角度和角速度的pid控制值
+  // 电机转轴角度平均数(上面计算)-位移零点=实际位移与零点的差值, 目标值是零
+  // 计算位移误差，并通过位移 PID 控制器得到位移控制量
+  distance_control = pid_distance(LQR_distance - distance_zeropoint); 
+  // 电机转速平均数(上面计算)-速度目标值=实际转速与目标值的差值, 目标值是零
+  // 计算速度误差（实际速度减去经过低通滤波和缩放后的遥控器期望速度），并通过速度 PID 控制器得到速度控制量
+  speed_control = pid_speed(LQR_speed - 0.1 * lpf_joyy(wrobot.joyy));
+
+  // 轮部离地检测, 连续记录两次的轮部转速
+  robot_speed_last = robot_speed;
+  robot_speed = LQR_speed;
+  // 若轮部角速度、角加速度"过大"（ > 10或> 50）,或处于跳跃后的恢复时期（jump_flag != 0），认为出现轮部离地现象，需要特殊处理
+  //轮部离地情况下 LQR_u计算：2个参数，角度控制量+角速度控制量，对轮位移(distance)和速度(speed)控制分量不输出；反之，正常状态下完整输出平衡转矩
+  if (abs(robot_speed - robot_speed_last) > 10 || abs(robot_speed) > 50 || (jump_flag != 0))  // 轮部离地时
+  {
+    distance_zeropoint = LQR_distance;    // 位移零点重置
+    // 位移和速度控制分量被忽略
+    LQR_u = angle_control + gyro_control; // 轮部离地情况下，对轮部分量不输出；反之，正常状态下完整输出平衡转矩
+    pid_lqr_u.error_prev = 0;             // 输出积分清零
+  }
+  else  // 当轮部未离地时，LQR_u：4个参数
+  {
+    // 当轮部未离地时，LQR_u 包含:角度控制量、角速度控制量、位移控制量和速度控制量，以实现完整的自平衡控制
+    LQR_u = angle_control + gyro_control + distance_control + speed_control;
+    // 计算得到LQR_u在loop()函数中在几个子loop执行完，被motor1和motor2执行
+    
+  }
+  // 小车没有控制的时候自稳定状态
+  // 控制量lqr_u<5，控制量小， 遥控器无信号输入joyy=0，轮部位移控制正常介入distance_control<4，不处于跳跃后的恢复时期jump_flag=0
+  if (abs(LQR_u) < 5 && wrobot.joyy == 0 && abs(distance_control) < 4 && (jump_flag == 0))
+  {
+
+    LQR_u = pid_lqr_u(LQR_u); // 小转矩非线性补偿
+    // Serial.println(LQR_u);
+
+    // lpf_zeropoint 是一个低通滤波器，对 distance_control 进行滤波处理，减少高频噪声的影响。
+    // 然后，将滤波后的 distance_control 作为输入，通过 pid_zeropoint 控制器进行进一步处理。
+    // angle_zeropoint -= ...：将计算得到的调整量从 angle_zeropoint 中减去，实现重心自适应调整。
+    // angle_zeropoint 是预设的角度零点，调整它可以让小车更好地适应不同的重心位置。
+    angle_zeropoint -= pid_zeropoint(lpf_zeropoint(distance_control)); // 重心自适应
+  }
+  else
+  {
+    pid_lqr_u.error_prev = 0; // 输出积分清零
+  }
+
+  if (obstacleDetected) {
+    // 首次进入障碍状态时记录时间
+    if (boostStartTime == 0) {
+      boostStartTime = millis();
+    }
+    
+    // 补偿时长未超时且机器人未严重倾斜时，应用扭矩补偿
+    if (millis() - boostStartTime < BOOST_DURATION) {
+      // blinkLed();
+      // 叠加补偿扭矩（仅正向，因为是前进障碍）
+      LQR_u = LQR_u * 10.5f;
+    }
+  } else {
+    // 无障碍物时重置补偿计时器并使用基础扭矩
+    boostStartTime = 0;
+  }
+
+  // 平衡控制参数自适应
+  if (wrobot.height < 50)
+  {
+    pid_speed.P = 0.7;
+  }
+  else if (wrobot.height < 64)
+  {
+    pid_speed.P = 0.6;
+  }
+  else
+  {
+    pid_speed.P = 0.5;
+  }
+
+}
+
+// 腿部动作控制（舵机）
+void leg_loop()
+{
+
+  // 倒地时强制腿部收缩
+  if (LQR_angle < -36.0f ||  LQR_angle > 45.0f) 
+  {
+      Position[0] = SERVO1_MIN;
+      Position[1] = SERVO2_MIN;
+      sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
+      return; // 跳过后续控制
+  }
+
+  jump_loop();
+
+  if (jump_flag == 0) // 不处于跳跃状态
+  {
+    // 舵机加速度
+    ACC[0] = 8;
+    ACC[1] = 8;
+
+    // 舵机速度
+    Speed[0] = 300;
+    Speed[1] = 300;
+    float roll_angle = (float)mpu6050.getAngleX() + 2.0;
+    leg_position_add = pid_roll_angle(lpf_roll(roll_angle));
+
+    int roll_offset = wrobot.roll;
+    int left_height, right_height;
+
+    // Serial.print("roll_offset: ");
+    // Serial.println(roll_offset);
+
+    // 如果控制了 roll 角度，则调整左右脚的高度
+    // 计算三角函数值
+    float angleRad = abs(roll_offset) * (pi / 180.0); // 将角度转换为弧度
+    float heightAdjust = 30 * sin(angleRad); // 小车宽度的一半是30毫米，抬高的高度是30毫米*sin(弧度)
+    float RollOffset = 18; // 所以水平高度需要加上调整高度，否则向下倾斜无法向下了
+    if (roll_offset > 10) {
+      // 正数，左脚高度 60，右脚高度 40
+      // 因为车身高度最小32，所以水平高度需要加上调整高度15，否则线下倾斜无法向下了
+      left_height = wrobot.height + RollOffset + heightAdjust;
+      right_height = wrobot.height + RollOffset - heightAdjust;
+      leg_position_add = 0; // 不使用roll角度 pid计算
+    } else if (roll_offset < -10) {
+      // 负数，左脚高度 40，右脚高度 60
+      left_height = wrobot.height + RollOffset - heightAdjust;
+      right_height = wrobot.height + RollOffset + heightAdjust;
+      leg_position_add = 0;
+    } else { // 未控制 roll 角度
+      // roll 为 0，左右恢复到同一高度
+      left_height = wrobot.height;
+      right_height = wrobot.height;
+      // 没有人为控制roll角度，leg_position_add根据mpu6050 roll值 进行pid计算得到，以保持小车机身平稳
+      // 但如果小车倒地，则不需要keep roll角度,否则小车倒在地上，1个脚长，1个脚短，很难看
+      if (abs(LQR_angle) > 25.0f) 
+      {
+        leg_position_add = 0;
+      }
+    }
+
+
+    // 根据新的高度计算舵机位置
+    // id 1 [0] 正面看右手，id 2 正面看左手 [1]
+    // 舵机id 1：1900（机身最矮）  2400（机身最高）2400-1900=500
+    // 舵机id 2：2300（机身最矮）  1800（机身最高）2300-1800=500
+    // 2048是舵机中间位置，12是微调偏移量，8.4是系数，32是默认高度，最高80，最低32
+    Position[0] = 2048 + 12 + 8.4 * (left_height - 32) - leg_position_add;
+    Position[1] = 2048 - 12 - 8.4 * (right_height - 32) - leg_position_add;
+    // Position[0] = 2048 + 12 + 8.4 * (left_height - 52) - leg_position_add;  // -52 机身下降20
+    // Position[1] = 2048 - 12 - 8.4 * (right_height - 52) - leg_position_add;
+    // Position[0] = 2048 + 12 + 8.4 * (left_height - 0) - leg_position_add; // 不减32 机身高度默认抬高32
+    // Position[1] = 2048 - 12 - 8.4 * (right_height - 0) - leg_position_add;
+
+    // Serial.print("left_height: ");
+    // Serial.print(left_height);
+    // Serial.print(", right_height: ");
+    // Serial.print(right_height);
+    // Serial.print(", leg_position_add: ");
+    // Serial.println(leg_position_add);
+
+
+    // 限制舵机位置范围
+    if (Position[0] < SERVO1_MIN) // 舵机1最低
+      Position[0] = SERVO1_MIN;
+    else if (Position[0] > SERVO1_MAX) // 舵机1最高
+      Position[0] = SERVO1_MAX;
+
+    if (Position[1] > SERVO2_MIN) // 舵机2最低（和舵机1方向是反的）
+      Position[1] = SERVO2_MIN;
+    else if (Position[1] < SERVO2_MAX) // 舵机2最高
+      Position[1] = SERVO2_MAX;
+    
+    sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
+  }
+}
+
+// 跳跃控制
+void jump_loop()
+{ // dir: 4 = STOP, 5 = jump 上次动作是跳跃5，本次是停止4，才开始跳，因为可能连续发送了很多5跳跃指令，只执行最后一次
+  // 跳跃，腿部以最快速度伸到最长
+  if ((wrobot.dir_last == 5) && (wrobot.dir == 4) && (jump_flag == 0))
+  { // 跳跃开始
+    ACC[0] = 0;
+    ACC[1] = 0;
+    Speed[0] = 0;
+    Speed[1] = 0;
+    Position[0] = SERVO1_MAX; 
+    Position[1] = SERVO2_MAX;
+    sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
+
+    jump_flag = 1;
+  }
+  if (jump_flag > 0) // 跳跃中，腿部缩起来，等待30帧，对应上面设置 jump_flag = 1;
+  {
+    jump_flag++;
+    if ((jump_flag > 30) && (jump_flag < 35))
+    {
+      ACC[0] = 0;
+      ACC[1] = 0;
+      Speed[0] = 0;
+      Speed[1] = 0;
+      Position[0] = 2048 + 12 + 8.4 * (35 - 32); // 降低腿部高度，即车重心
+      Position[1] = 2048 - 12 - 8.4 * (35 - 32);
+      sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
+
+      jump_flag = 40;
+    }
+    if (jump_flag > 200)
+    {
+      jump_flag = 0; // 跳跃过程结束
+    }
+  }
+}
+
+// yaw轴转向控制 （转向控制）
+void yaw_loop()
+{
+  // YAW_output = 0.03*(YAW_Kp*YAW_angle_total + YAW_Kd*YAW_gyro);
+  yaw_angle_addup();
+
+  YAW_angle_total += wrobot.joyx * 0.008; // 0.002
+  float yaw_angle_control = pid_yaw_angle(YAW_angle_total);
+  float yaw_gyro_control = pid_yaw_gyro(YAW_gyro);
+  //  yaw 轴的角度和角速度数据进行了 PID 控制，得到 yaw_angle_control 和 yaw_gyro_control。
+  // 然后，将这两个控制量相加，得到最终的 YAW_output。
+  YAW_output = yaw_angle_control + yaw_gyro_control;
+}
+
+// Web数据更新
+void web_loop()
+{
+  webserver.handleClient();
+  websocket.loop();
+  rp.spinOnce(); // 更新web端回传的控制信息
+}
+
+// yaw轴角度累加函数
+void yaw_angle_addup()
+{
+  YAW_angle = (float)mpu6050.getAngleZ();
+  YAW_gyro = (float)mpu6050.getGyroZ();
+
+  if (YAW_angle_zero_point == (-10))
+  {
+    YAW_angle_zero_point = YAW_angle;
+  }
+
+  float yaw_angle_1, yaw_angle_2, yaw_addup_angle;
+  if (YAW_angle > YAW_angle_last)
+  {
+    yaw_angle_1 = YAW_angle - YAW_angle_last;
+    yaw_angle_2 = YAW_angle - YAW_angle_last - 2 * PI;
+  }
+  else
+  {
+    yaw_angle_1 = YAW_angle - YAW_angle_last;
+    yaw_angle_2 = YAW_angle - YAW_angle_last + 2 * PI;
+  }
+
+  if (abs(yaw_angle_1) > abs(yaw_angle_2))
+  {
+    yaw_addup_angle = yaw_angle_2;
+  }
+  else
+  {
+    yaw_addup_angle = yaw_angle_1;
+  }
+
+  YAW_angle_total = YAW_angle_total + yaw_addup_angle;
+  YAW_angle_last = YAW_angle;
+}
+
+void basicWebCallback(void)
+{
+  webserver.send(300, "text/html", basic_web);
+}
+
+void webSocketEventCallback(uint8_t num, WStype_t type, uint8_t *payload, size_t length)
+{
+  if (type == WStype_TEXT)
+  {
+    String payload_str = String((char *)payload);
+    StaticJsonDocument<300> doc;
+    DeserializationError error = deserializeJson(doc, payload_str);
+
+    String mode_str = doc["mode"];
+    if (mode_str == "basic")
+    {
+      rp.parseBasic(doc);
+    }
+  }
+}
+
+// 电压检测初始化
+void adc_calibration_init()
+{
+  if (esp_adc_cal_check_efuse(ESP_ADC_CAL_VAL_EFUSE_TP) == ESP_OK)
+  {
+    printf("eFuse Two Point: Supported\n");
+  }
+  else
+  {
+    printf("eFuse Two Point: NOT supported\n");
+  }
+  // Check Vref is burned into eFuse
+  if (esp_adc_cal_check_efuse(ESP_ADC_CAL_VAL_EFUSE_VREF) == ESP_OK)
+  {
+    printf("eFuse Vref: Supported\n");
+  }
+  else
+  {
+    printf("eFuse Vref: NOT supported\n");
+  }
+}
+
+// 电压检测
+void bat_check()
+{
+  if (bat_check_num > 1000)
+  {
+    // 电压读取
+    uint32_t sum = 0;
+    sum = analogRead(BAT_PIN);
+    uint32_t voltage = esp_adc_cal_raw_to_voltage(sum, &adc_chars);
+    double battery = (voltage * 3.97) / 1000.0;
+
+    // Serial.println(battery);
+    // 电量显示
+    if (battery > 7.8)
+      digitalWrite(LED_BAT, HIGH);
+    else
+      digitalWrite(LED_BAT, LOW);
+
+    bat_check_num = 0;
+  }
+  else
+    bat_check_num++;
+}
+
+// LED闪烁函数(不适用for语气，非阻塞)
+void blinkLed() {
+  unsigned long currentTime = millis();
+  if (currentTime - lastBlinkTime > BLINK_INTERVAL) {
+    lastBlinkTime = currentTime;
+    digitalWrite(LED_BAT, !digitalRead(LED_BAT)); // 切换LED状态
+    
+    if (++blinkCount >= BLINK_TIMES * 2) { // 每次闪烁包含开和关
+      blinking = false;
+      digitalWrite(LED_BAT, LOW); // 结束闪烁后保持熄灭
+    }
+  }
+}
+
+// 障碍物检测函数
+void detectObstacle() {
+  // 条件1: 有前进指令(joyy > 0)且超过死区
+  // 条件2: 实际速度很低(接近零)
+  // 条件3: 不在跳跃状态
+  bool hasForwardCommand = (wrobot.joyy > deadZone);
+  bool isAlmostStopped = (abs(LQR_speed) < 1.0);
+  bool notJumping = (jump_flag == 0);
+  bool fallAngle = (LQR_angle > 5.0f || LQR_angle < 40.0f);
+  
+  if (hasForwardCommand && isAlmostStopped && notJumping && fallAngle) {
+    // 如果符合条件且尚未计时，记录开始时间
+    if (obstacleStartTime == 0) {
+      obstacleStartTime = millis();
+    } 
+    // 如果持续时间超过阈值，判定为遇到障碍物
+    else if (millis() - obstacleStartTime > OBSTACLE_DELAY) {
+      obstacleDetected = true;
+      // blinkLed();
+    }
+  } else {
+    // 不符合条件，重置状态
+    obstacleDetected = false;
+    obstacleStartTime = 0;
+  }
+}

Shibo-main-TOMMY/test/main.cpp

@@ -151,16 +151,6 @@ void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
   // 取反按钮值，使得默认显示 0 false，按下显示 1 true
   int convertedbuttonLB = !remoteData.buttonLB; // 顶部左
   int convertedbuttonRB = !remoteData.buttonRB; // 顶部右
-  int convertedbuttonR1 = !remoteData.buttonR1; // 底部右
-  int convertedbuttonR2 = !remoteData.buttonR2; // 底部左
-
-  // 改变倾角
-  // if(convertedbuttonR1){
-  //   angle_zeropoint++;
-  // }
-  // if(convertedbuttonR2){
-  //   angle_zeropoint--;
-  // }
 
   // 跳跃
   if (convertedbuttonRB) { // 1=true，0=false
@@ -312,20 +302,37 @@ void loop()
   wrobot.joyx_last = wrobot.joyx;
   wrobot.joyy_last = wrobot.joyy;
 
-  // 将自平衡计算输出转矩赋给电机
+  /* 将自平衡计算输出转矩赋给电机
-  // motor.target 负数小车向前转，正数小车向后转，
+  motor.target 负数小车向前转，正数小车向后转，
+  LQR_u 越大，电机输出的转矩越大，电机向后转，默认0-8V
+  LQR_u = angle_control + gyro_control + distance_control + speed_control
+
+   angle_control = pid_angle(LQR_angle - angle_zeropoint) //LQR_u与实际LQR_angle正相关
+                 = pid_angle( mpu6050.getAngleY() - angle_zeropoint) //LQR_u与实际pitch正相关
+   gyro_control = pid_gyro(LQR_gyro) //LQR_u与实际LQR_gyro正相关
+                = pid_gyro( mpu6050.getGyroY() ) //LQR_u与实际pitch 角速度 正相关
+    
+    所以这里LQR_distance和LQR_speed的计算乘以-0.5,不能单看负号
+   distance_control = pid_distance(LQR_distance - distance_zeropoint) //LQR_u与电LQR_distance正相关 
+                    = pid_distance( (-0.5)*(motor1.shaft_angle + motor2.shaft_angle) - distance_zeropoint)  
+                    //在平衡车中，通常设定电机向后转（使车前进）为负，所以LQR_u与电机实际shaft_angle角度正相关，和负号没关系
+   
+    speed_control = pid_speed(LQR_speed - 0.1 * lpf_joyy(wrobot.joyy)) // 实际速度-目标速度
+                  //LQR_speed代表实际速度，LQR_u与实际LQR_speed速度正相关
+                  = pid_speed( (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity) - 0.1 * lpf_joyy(wrobot.joyy)) 
+                  //LQR_u与实际shaft_velocity速度正相关
+   */
   motor1.target = (-0.5) * (LQR_u + YAW_output); // 负数小车向前，正数小车向后，取
   motor2.target = (-0.5) * (LQR_u - YAW_output);
 
   // 查看角度信息, 屁股向后倒 -33~-34度,舵机2048的时候-42度，头向前到底 75度，完全垂直88度
   //  Serial.println(LQR_angle);
-   Serial.println(angle_zeropoint);
 
   // 检测小车是否向后马上要倒地了，在中间过程重置所有参数
   loopCounter++;
-  // loopCounter作用：在刚开机的800次loop循环，不能进入resetToDefault()函数
+  // loopCounter作用：在期初的800次循环内视为电源开通后第一次自平衡，不能进入resetToDefault()函数重置参数
-  // resetToDefaultCounter作用：防止小车倒地后某个角度，陷入循环，motor.target一直等于0，就躺地上不动了
+  // resetToDefaultCounterz作用：防止小车倒地后某个角度，陷入循环，motor.target一直=0
-  if ((LQR_angle < -35.0f || LQR_angle > 70.0f )&& loopCounter > 800 && resetToDefaultCounter < 130) {
+  if ((LQR_angle < -35.0f || LQR_angle > 70.0f ) && loopCounter > 800 && resetToDefaultCounter < 130) {
       resetToDefault();
       resetToDefaultCounter++;
       motor1.target = 0;
@@ -369,31 +376,9 @@ void lqr_balance_loop()
         // + LQR_k4*LQR_speed;
 
   // 给负值是因为按照当前的电机接线，正转矩驱动小车电机向后转
-
+  // 编码器反馈shaft_angle角度，shaft_velocity速度
-  /*
+  // 不操作前后移动，譬如刚开机，车轮不动的，shaft_angle和shaft_velocity 都是零
-  LQR_u 越大，电机输出的转矩越大，电机向后转，默认0-8V
+  // 所以，不操作前后移动，LQR_distance 和 LQR_speed趋近于0，只有角度控制和角速度控制
-  LQR_u = angle_control + gyro_control + distance_control + speed_control
-
-  编码器反馈shaft_angle角度，shaft_velocity速度
-  不操作前后移动，譬如刚开机，车轮不动的，shaft_angle和shaft_velocity 都是零
-  所以，不操作前后移动，LQR_distance 和 LQR_speed趋近于0，只有角度控制和角速度控制
-
-   angle_control = pid_angle(LQR_angle - angle_zeropoint) //LQR_u与实际LQR_angle正相关
-                 = pid_angle( mpu6050.getAngleY() - angle_zeropoint) //LQR_u与实际pitch正相关
-   gyro_control = pid_gyro(LQR_gyro) //LQR_u与实际LQR_gyro正相关
-                = pid_gyro( mpu6050.getGyroY() ) //LQR_u与实际pitch 角速度 正相关
-    
-    所以这里LQR_distance和LQR_speed的计算乘以-0.5,不能单看负号
-   distance_control = pid_distance(LQR_distance - distance_zeropoint) //LQR_u与电LQR_distance正相关 
-                    = pid_distance( (-0.5)*(motor1.shaft_angle + motor2.shaft_angle) - distance_zeropoint)  
-                    //在平衡车中，通常设定电机向后转（使车前进）为负，所以LQR_u与电机实际shaft_angle角度正相关，和负号没关系
-   
-    speed_control = pid_speed(LQR_speed - 0.1 * lpf_joyy(wrobot.joyy)) // 实际速度-目标速度
-                  //LQR_speed代表实际速度，LQR_u与实际LQR_speed速度正相关
-                  = pid_speed( (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity) - 0.1 * lpf_joyy(wrobot.joyy)) 
-                  //LQR_u与实际shaft_velocity速度正相关
-  */
-
   // 在平衡车中，通常设定电机向后转（使车前进）为负，所以这里LQR_distance和LQR_speed的计算乘以-0.5
   LQR_distance = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); // 两个电机的旋转角度（shaft_angle）（是位置量）的平均值
   LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity); // 两个电机角速度（shaft_velocity）的平均值，并取反
@@ -583,6 +568,14 @@ void jump_loop()
   // 跳跃，腿部以最快速度伸到最长
   if ((wrobot.dir_last == 5) && (wrobot.dir == 4) && (jump_flag == 0))
   { 
+    // 设定最大正向电压（快速加速）
+    // motor1.target = -8.0f; 
+    // motor2.target = -8.0f;
+    // motor1.loopFOC();
+    // motor2.loopFOC();
+    // motor1.move();
+    // motor2.move();
+    // delay(50); // 缩短加速时间（50ms足够获得初速度）
 
     // 跳跃开始
     ACC[0] = 0;
@@ -600,6 +593,10 @@ void jump_loop()
   {
     jump_flag++;
 
+    // if (jump_flag == 25) {  // 跳跃上升阶段停止轮子转动
+    //   motor1.target = 0;
+    //   motor2.target = 0;
+    // }
 
     if ((jump_flag > 30) && (jump_flag < 35))
     {

Shibo-main-TOMMY/test/web.html

@@ -0,0 +1,549 @@
+<!DOCTYPE html>
+<html lang="en">
+<head>
+    <meta charset="UTF-8">
+    <meta name="viewport" content="width=device-width, initial-scale=1.0">
+    <title>控制器</title>
+    <style>
+    *{box-sizing:border-box;margin:0;padding:0}
+    body{display:flex;flex-direction:column;align-items:center;justify-content:center;min-height:100vh;background-color:#fff;font-family:'Segoe UI',Arial,sans-serif;padding:20px}
+    .page-title{color:#333;margin-bottom:20px;text-align:center;font-weight:600;font-size:1.5rem}
+    
+    /* 摇杆容器 - 关键修改：使用flex-wrap并设置固定间距 */
+    .joysticks-container{
+        display:flex;
+        gap:15px; /* 摇杆之间的间距 */
+        align-items:center;
+        justify-content:center;
+        flex-wrap:wrap; /* 允许换行 */
+        width:100%;
+        max-width:1200px;
+        padding:10px 0;
+    }
+    
+    /* 滚动条美化 */
+    .joysticks-container::-webkit-scrollbar{height:6px;}
+    .joysticks-container::-webkit-scrollbar-thumb{background-color:#4a90e2;border-radius:3px;}
+    .joysticks-container::-webkit-scrollbar-track{background-color:#f1f1f1;border-radius:3px;}
+    
+    /* 摇杆项 - 关键修改：固定为50%宽度（减去间距） */
+    .joystick-item{
+        display:flex;
+        flex-direction:column;
+        align-items:center;
+        gap:10px;
+        width:calc(50% - 10px); /* 50%宽度减去一半间距 */
+        min-width:unset; /* 移除最小宽度限制 */
+        padding:0 5px;
+    }
+    
+    .joystick-label{
+        color:#333;
+        font-weight:500;
+        font-size:14px;
+        text-align:center;
+        padding:3px 8px;
+        border-radius:4px;
+        background-color:rgba(255,255,255,.8);
+        white-space:nowrap;
+    }
+    
+    .joystick-wrapper{
+        display:flex;
+        position:relative;
+        width:100%; /* 占满父容器宽度 */
+        justify-content:center;
+    }
+    
+    .joystick-wrapper.vertical{flex-direction:column;align-items:center;}
+    .joystick-wrapper.horizontal{flex-direction:row;align-items:center;}
+    
+    .joystick-track{
+        position:relative;
+        border:2px solid #4a90e2;
+        border-radius:30px;
+        overflow:hidden;
+    }
+    
+    .joystick-track:hover{box-shadow:0 6px 16px rgba(0,0,0,.15);}
+    
+    /* 轨道尺寸 - 水平轨道占满50%屏幕宽度，垂直轨道固定宽度 */
+    .joystick-track.vertical{width:60px;height:300px;}
+    .joystick-track.horizontal{width:100%;max-width:300px;height:60px;}
+    
+    .joystick-handle{
+        position:absolute;
+        width:50px;
+        height:50px;
+        background-color:#4a90e2;
+        border-radius:50%;
+        cursor:grab;
+        transition:transform .1s ease-out,background-color .2s ease;
+        user-select:none;
+        -webkit-user-select:none;
+        border:2px solid rgba(255,255,255,.7);
+        display:flex;
+        align-items:center;
+        justify-content:center;
+        color:white;
+        font-weight:600;
+        font-size:14px;
+    }
+    
+    .joystick-handle:active{
+        cursor:grabbing;
+        background-color:#357abd;
+        transform:scale(.95);
+    }
+    
+    .joystick-wrapper.vertical .joystick-handle{left:5px;top:125px;}
+    .joystick-wrapper.horizontal .joystick-handle{left:calc(50% - 25px);top:5px;} /* 水平居中 */
+    
+    .usage-info{
+        margin-top:30px;
+        color:#666;
+        font-size:14px;
+        text-align:center;
+        max-width:600px;
+        line-height:1.5;
+    }
+    
+    /* 平板设备适配 */
+    @media (max-width:1024px){
+        .joystick-track.vertical{width:50px;height:250px;}
+        .joystick-track.horizontal{height:50px;}
+        .joystick-handle{width:40px;height:40px;font-size:12px;}
+        .joystick-wrapper.vertical .joystick-handle{left:5px;top:105px;}
+        .joystick-wrapper.horizontal .joystick-handle{left:calc(50% - 20px);top:5px;}
+    }
+    
+    /* 手机设备适配 - 确保2个一行 */
+    @media (max-width:768px){
+        body{padding:10px 5px;}
+        .page-title{margin-bottom:15px;font-size:1.2rem;}
+        .joysticks-container{gap:10px;}
+        .joystick-item{width:calc(50% - 5px);} /* 调整间距计算 */
+        .joystick-track.vertical{width:45px;height:200px;}
+        .joystick-handle{width:35px;height:35px;font-size:12px;}
+        .joystick-wrapper.vertical .joystick-handle{left:5px;top:82.5px;}
+        .joystick-wrapper.horizontal .joystick-handle{left:calc(50% - 17.5px);top:5px;}
+    }
+    
+    /* 小屏手机额外适配 */
+    @media (max-width:375px){
+        .joystick-track.vertical{width:40px;height:180px;}
+        .joystick-handle{width:30px;height:30px;font-size:11px;}
+        .joystick-wrapper.vertical .joystick-handle{left:5px;top:75px;}
+        .joystick-wrapper.horizontal .joystick-handle{left:calc(50% - 15px);top:5px;}
+        .joystick-label{font-size:12px;}
+    }
+    
+    .button-container {
+        display: flex;
+        gap: 50px; 
+        margin-bottom: 20px;
+        width: 100%;
+        justify-content: center;
+    }
+    
+    .control-btn {
+        padding: 12px 24px;
+        background: #fff;
+        color: #2196F3;
+        border: 2px solid #2196F3;
+        border-radius: 6px;
+        cursor: pointer;
+        transition: all 0.3s;
+    }
+    
+    .control-btn:active {
+        background-color: #2196F3;
+        color: white;
+    }
+    
+    .connection-status {
+        margin-bottom: 15px;
+        padding: 6px 12px;
+        border-radius: 4px;
+        font-weight: 500;
+        font-size: 14px;
+    }
+    
+    .status-connected {
+        background-color: #e8f5e9;
+        color: #2e7d32;
+    }
+    
+    .status-disconnected {
+        background-color: #ffebee;
+        color: #c62828;
+    }
+    </style>
+</head>
+<body onload="socket_init()">
+    <h1 class="page-title">控制器</h1>
+    
+    <div class="connection-status status-disconnected" id="connectionStatus">未连接</div>
+    
+    <div class="button-container">
+        <button class="control-btn" id="blink">blink</button>
+        <button class="control-btn" id="dir">jump</button>
+    </div>
+    
+    <div class="joysticks-container">
+        <!-- 第一行第一个：水平摇杆 -->
+        <div class="joystick-item">
+            <div class="joystick-label">摇摆</div>
+            <div id="roll" class="joystick-wrapper horizontal"></div>
+        </div>
+        
+        <!-- 第一行第二个：水平摇杆 -->
+        <div class="joystick-item">
+            <div class="joystick-label">左右</div>
+            <div id="joy_x" class="joystick-wrapper horizontal"></div>
+        </div>
+        
+        <!-- 第二行第一个：垂直摇杆 -->
+        <div class="joystick-item">
+            <div class="joystick-label">高度</div>
+            <div id="height" class="joystick-wrapper vertical"></div>
+        </div>
+        
+        <!-- 第二行第二个：垂直摇杆 -->
+        <div class="joystick-item">
+            <div class="joystick-label">前后</div>
+            <div id="joy_y" class="joystick-wrapper vertical"></div>
+        </div>
+    </div>
+    
+    <script>
+        var socket; // WebSocket实例
+        var isConnected = false; // 连接状态
+        
+        // 更新连接状态显示
+        function updateConnectionStatus(connected) {
+            isConnected = connected;
+            const statusElement = document.getElementById('connectionStatus');
+            if (connected) {
+                statusElement.textContent = '已连接';
+                statusElement.className = 'connection-status status-connected';
+            } else {
+                statusElement.textContent = '未连接';
+                statusElement.className = 'connection-status status-disconnected';
+            }
+        }
+        
+        // 初始化WebSocket连接
+        function socket_init() {
+            try {
+                // 根据环境选择合适的连接方式
+                const wsProtocol = window.location.protocol === 'https:' ? 'wss:' : 'ws:';
+                socket = new WebSocket(wsProtocol + '//' + window.location.hostname + ':81/');
+                
+                // 连接成功事件
+                socket.onopen = function() {
+                    console.log('WebSocket连接已建立');
+                    updateConnectionStatus(true);
+                };
+                
+                // 接收消息事件
+                socket.onmessage = function(event) {
+                    console.log('收到消息:', event.data);
+                    // 可以在这里处理从服务器返回的消息
+                };
+                
+                // 连接关闭事件
+                socket.onclose = function() {
+                    console.log('WebSocket连接已关闭');
+                    updateConnectionStatus(false);
+                    // 尝试重连
+                    setTimeout(socket_init, 3000);
+                };
+                
+                // 错误事件
+                socket.onerror = function(error) {
+                    console.error('WebSocket错误:', error);
+                    updateConnectionStatus(false);
+                };
+            } catch (e) {
+                console.error('WebSocket初始化失败:', e);
+                updateConnectionStatus(false);
+                // 重试连接
+                setTimeout(socket_init, 3000);
+            }
+        }
+
+        // 摇杆控制器类
+        class Joystick {
+            constructor(containerId, direction = 'vertical', min = -100, max = 100) {
+                this.container = document.getElementById(containerId);
+                this.direction = direction;
+                this.id = containerId;
+                this.isDragging = false;
+                this.startPos = 0;
+                this.currentPos = 0;
+                this.centerPos = 0;
+                this.min = min;
+                this.max = max;
+                this.range = max - min;
+                
+                this.createElements();
+                this.handle = this.container.querySelector('.joystick-handle');
+                this.track = this.container.querySelector('.joystick-track');
+                this.initDimensions();
+                this.bindEvents();
+                this.setHandlePosition(this.centerPos);
+            }
+            
+            createElements() {
+                const track = document.createElement('div');
+                track.className = `joystick-track ${this.direction}`;
+                
+                const handle = document.createElement('div');
+                handle.className = 'joystick-handle';
+                handle.textContent = '0';
+                
+                track.appendChild(handle);
+                this.container.appendChild(track);
+            }
+            
+            initDimensions() {
+                if (this.direction === 'vertical') {
+                    this.trackSize = this.track.offsetHeight;
+                    this.handleSize = this.handle.offsetHeight;
+                    this.centerPos = (this.trackSize - this.handleSize) / 2;
+                } else {
+                    this.trackSize = this.track.offsetWidth;
+                    this.handleSize = this.handle.offsetWidth;
+                    this.centerPos = (this.trackSize - this.handleSize) / 2;
+                }
+            }
+            
+            clamp(value, min, max) {
+                return Math.min(Math.max(value, min), max);
+            }
+            
+            updateValue(position) {
+                const offset = this.direction === 'vertical' 
+                    ? this.centerPos - position 
+                    : position - this.centerPos;
+                
+                const rawPercentage = (offset / this.centerPos) * 100;
+                const rawValue = this.clamp(Math.round(rawPercentage), -100, 100);
+
+                const mappedValue = Math.round(
+                    ((rawValue + 100) / 200) * this.range + this.min
+                );
+
+                this.handle.textContent = mappedValue;
+                this.dispatchValueChange({raw: rawValue, mapped: mappedValue});
+            }
+            
+            dispatchValueChange({raw, mapped}) {
+                const event = new CustomEvent('valuechange', {
+                    detail: {
+                        rawValue: raw,
+                        mappedValue: mapped,
+                        id: this.id,
+                        joystick: this
+                    },
+                    bubbles: true
+                });
+                this.container.dispatchEvent(event);
+            }
+            
+            setHandlePosition(pos) {
+                const minPos = 0;
+                const maxPos = this.trackSize - this.handleSize;
+                const constrainedPos = this.clamp(pos, minPos, maxPos);
+                
+                if (this.direction === 'vertical') {
+                    this.handle.style.top = `${constrainedPos}px`;
+                } else {
+                    this.handle.style.left = `${constrainedPos}px`;
+                }
+                
+                this.updateValue(constrainedPos);
+            }
+            
+            returnToCenter() {
+                if (!this.isDragging) {
+                    const currentPosition = parseFloat(
+                        this.direction === 'vertical' 
+                            ? this.handle.style.top 
+                            : this.handle.style.left
+                    ) || this.centerPos;
+                    
+                    const distance = this.centerPos - currentPosition;
+                    
+                    if (Math.abs(distance) < 1) {
+                        this.setHandlePosition(this.centerPos);
+                        return;
+                    }
+                    
+                    const step = distance * 0.5;
+                    this.setHandlePosition(currentPosition + step);
+                    requestAnimationFrame(() => this.returnToCenter());
+                }
+            }
+            
+            bindEvents() {
+                this.handle.addEventListener('mousedown', (e) => {
+                    this.isDragging = true;
+                    this.startPos = this.direction === 'vertical' ? e.clientY : e.clientX;
+                    this.currentPos = parseFloat(
+                        this.direction === 'vertical' 
+                            ? this.handle.style.top 
+                            : this.handle.style.left
+                    ) || this.centerPos;
+                    e.preventDefault();
+                });
+                
+                document.addEventListener('mousemove', (e) => {
+                    if (this.isDragging) {
+                        const delta = (this.direction === 'vertical' ? e.clientY : e.clientX) - this.startPos;
+                        this.setHandlePosition(this.currentPos + delta);
+                    }
+                });
+                
+                document.addEventListener('mouseup', () => {
+                    if (this.isDragging) {
+                        this.isDragging = false;
+                        this.returnToCenter();
+                    }
+                });
+                
+                this.handle.addEventListener('touchstart', (e) => {
+                    this.isDragging = true;
+                    this.startPos = this.direction === 'vertical' 
+                        ? e.touches[0].clientY 
+                        : e.touches[0].clientX;
+                    this.currentPos = parseFloat(
+                        this.direction === 'vertical' 
+                            ? this.handle.style.top 
+                            : this.handle.style.left
+                    ) || this.centerPos;
+                    e.preventDefault();
+                });
+                
+                document.addEventListener('touchmove', (e) => {
+                    if (this.isDragging) {
+                        const delta = (this.direction === 'vertical' 
+                            ? e.touches[0].clientY 
+                            : e.touches[0].clientX) - this.startPos;
+                        this.setHandlePosition(this.currentPos + delta);
+                        e.preventDefault();
+                    }
+                }, { passive: false });
+                
+                document.addEventListener('touchend', () => {
+                    if (this.isDragging) {
+                        this.isDragging = false;
+                        this.returnToCenter();
+                    }
+                });
+                
+                window.addEventListener('resize', () => {
+                    this.initDimensions();
+                    if (!this.isDragging) {
+                        this.setHandlePosition(this.centerPos);
+                    }
+                });
+            }
+        }
+        
+        // 控制状态对象
+        const controlState = {
+            blink: 0,
+            dir: "stop",
+            roll: 0,
+            height: 30,
+            joy_y: 0,
+            joy_x: 0
+        };
+
+        // 统一发送函数
+        function sendAllData() {
+            if (!isConnected) {
+                console.log('未连接，无法发送数据');
+                return;
+            }
+            
+            const data = {
+                dir: controlState.dir,
+                roll: controlState.roll,
+                height: controlState.height,
+                joy_y: controlState.joy_y,
+                joy_x: controlState.joy_x,
+                blink: controlState.blink,
+                stable: 1,
+                'mode': 'basic',
+            };
+            
+            try {
+                socket.send(JSON.stringify(data));
+                console.log('发送数据:', data);
+            } catch (e) {
+                console.error('发送数据失败:', e);
+                updateConnectionStatus(false);
+            }
+            
+            if (controlState.blink === 1) controlState.blink = 0;
+            if (controlState.dir === "jump") controlState.dir = "stop";
+        }
+
+        // 初始化摇杆实例
+        document.addEventListener('DOMContentLoaded', () => {
+            const joysticks = [
+                new Joystick('roll', 'horizontal', -30, 30),
+                new Joystick('height', 'vertical', 0, 60),
+                new Joystick('joy_y', 'vertical'),
+                new Joystick('joy_x', 'horizontal')
+            ];
+            
+            document.querySelectorAll('.joystick-wrapper').forEach(wrapper => {
+                wrapper.addEventListener('valuechange', (e) => {
+                    controlState[e.detail.id] = e.detail.mappedValue;
+                    sendAllData();
+                });
+            });
+            
+            document.querySelectorAll('.control-btn').forEach(btn => {
+                btn.addEventListener('mousedown', (e) => {
+                    btn.style.backgroundColor = '#2196F3';
+                    btn.style.color = 'white';
+                    
+                    switch(e.target.id) {
+                        case 'blink': controlState.blink = 1; break;
+                        case 'dir': controlState.dir = "jump"; break;
+                    }
+                    sendAllData();
+                });
+                
+                btn.addEventListener('mouseup', () => {
+                    btn.style.backgroundColor = '';
+                    btn.style.color = '';
+                });
+                
+                btn.addEventListener('touchstart', (e) => {
+                    btn.style.backgroundColor = '#2196F3';
+                    btn.style.color = 'white';
+                    
+                    switch(e.target.id) {
+                        case 'blink': controlState.blink = 1; break;
+                        case 'dir': controlState.dir = "jump"; break;
+                    }
+                    sendAllData();
+                    e.preventDefault();
+                });
+                
+                btn.addEventListener('touchend', (e) => {
+                    btn.style.backgroundColor = '';
+                    btn.style.color = '';
+                    e.preventDefault();
+                });
+            });
+        });
+    </script>
+</body>
+</html>