增加一个简单的越障模式和斜坡自平衡功能。

README.md

@@ -58,4 +58,4 @@ B站演示地址：https://space.bilibili.com/25336365
 
 2025-10-30 v1.2增加重力感应，主打身段要柔软。
 
-2025-11-22 v1.3增加一个简单的越障模式。遥控器右边的扳手键，根据按压程度，对不同高度进行越障。https://www.bilibili.com/video/BV1K5U4BBEwZ/。
+2025-11-23 v1.3增加一个简单的越障模式，和斜坡自平衡功能.

v1.3/src/SerialParser.cpp

@@ -0,0 +1,48 @@
+#include "SerialParser.h"  // 仅包含自己的头文件，无需 main.h
+
+// -------------------------- 全局变量定义（与 main.cpp 中原来的定义一致）--------------------------
+char serial_buf[32];  // 存储串口接收数据
+uint8_t buf_idx = 0;  // 缓冲区索引
+
+// -------------------------- 串口接收处理函数（原逻辑不变）--------------------------
+void serialReceiveProcess() {
+  while (Serial.available()) {
+    char c = Serial.read();
+    if (c == '\n' || c == '\r') {  // 指令结束符（回车/换行）
+      serial_buf[buf_idx] = '\0';  // 字符串终止
+      parseSingleParam(serial_buf); // 调用解析函数
+      buf_idx = 0;                 // 重置缓冲区索引
+    } else if (buf_idx < 31) {     // 防止缓冲区溢出（最多存31个字符）
+      serial_buf[buf_idx++] = c;   // 存入缓冲区
+    }
+  }
+}
+
+// -------------------------- 参数解析函数（原逻辑不变）--------------------------
+void parseSingleParam(char* cmd) {
+  char* eq_pos = strchr(cmd, '=');  // 找到等号位置
+  if (eq_pos == NULL) {             // 无等号则忽略
+    Serial.println("格式错误！需含\"=\"");
+    return;
+  }
+  *eq_pos = '\0';  // 分割参数名与数值（cmd=参数名，eq_pos+1=数值）
+  char* param_name = cmd;
+  float param_val = atof(eq_pos + 1);  // 转为浮点值
+
+  // 匹配参数名并赋值（访问全局对象，已在 SerialParser.h 中声明）
+  if (strcmp(param_name, "Kp") == 0) {
+    pid_super_balance.P = param_val;
+  } else if (strcmp(param_name, "Ki") == 0) {
+    pid_super_balance.I = param_val;
+  } else if (strcmp(param_name, "Kd") == 0) {
+    pid_super_balance.D = param_val;
+  } else if (strcmp(param_name, "zaccel_threshold") == 0) {
+    zaccel_threshold = param_val;
+  } else {
+    Serial.println("参数名错误！");
+    return;
+  }
+
+  // 反馈更新结果
+  Serial.printf("更新成功：%s=%.2f\n", param_name, param_val);
+}

v1.3/src/SerialParser.h

@@ -0,0 +1,21 @@
+#ifndef SERIAL_PARSER_H
+#define SERIAL_PARSER_H
+
+#include <Arduino.h>
+#include <SimpleFOC.h>  // 包含 PIDController 类的定义
+#include <FastLED.h>    // 包含 CRGB 类（如果用到 LED 相关）
+
+// -------------------------- 全局对象声明（与 main.cpp 中定义一致）--------------------------
+// 这些对象在 main.cpp 中定义，此处用 extern 声明供 SerialParser 访问
+extern PIDController pid_super_balance;
+extern float zaccel_threshold;
+
+// 串口缓冲区全局变量（定义在 SerialParser.cpp 中）
+extern char serial_buf[32];
+extern uint8_t buf_idx;
+
+// -------------------------- 函数声明 --------------------------
+void serialReceiveProcess();
+void parseSingleParam(char* cmd);
+
+#endif // SERIAL_PARSER_H

v1.3/src/main.cpp

@@ -12,20 +12,23 @@ Modify by Tommy 2025
 #include "VoltageMonitor.h"  // 电源
 #include "XboxBluetooth.h"   // 蓝牙手柄
 #include "RobotActions.h"    // 动作处理
+#include "SerialParser.h"  // 包含串口解析头文件
 
 
 /************函数申明*************/
+
 // 平衡算法函数
 void lqr_balance_loop();
 void yaw_loop();
 void leg_loop();
 void jump_loop();
 
+void cancel_super_mode();
+
 // 跨越障碍函数
 void obstacle_detect(); // 越障高度检测
 void obstacle_loop(); // 越障总函数
 
-
 // 常规运动控制
 void resetZeroPoint();
 void jump_charge(); // 腿部弯曲（蓄能）
@@ -97,6 +100,15 @@ MPU6050 mpu6050(I2Ctwo);
 /************参数定义*************/
 #define pi 3.1415927
 
+// 重力感应z轴加速度判断
+float zaccel_threshold = 1.8f; // 常规重力感应阈值
+float normal_zaccel_threshold = 1.8f; // 常规重力感应阈值
+float super_zaccel_threshold = 1.2f; //超平衡模式下阈值
+// 快速下坠加速度阈值
+bool is_falling = false; // 正在下降
+unsigned long last_falling_trigger_time = 0; // 上一次触发is_falling的时间戳（毫秒）
+const unsigned long FALLING_COOLDOWN = 3000; // 冷却时间：3000毫秒（3秒）
+
 // 机身平衡角度调整 roll轴
 float roll_adjust = 0.0f;  
 float original_roll_adjust = roll_adjust;  
@@ -185,7 +197,10 @@ int obstacle_flag_count = 0; // 越障标记计数
 int obstacle_pitch_adjust_count = 0; // 前倾控制
 int obstacle_motor_go_count = 0; // 电机前进计数
 
-int wheel_ground_flag = 1; // 轮子是否离地标记 默认离地
+
+// 超级平衡模式参数
+bool super_balance_mode = false; // 超级平衡模式
+PIDController pid_super_balance(10, 0, 0, 100000, 100); // 适配joyy实际需求
 
 // 舵机中位校准
 int calibrationOfs_flag = 0;
@@ -193,8 +208,7 @@ int calibrationOfs_flag = 0;
 // 倒地恢复计数
 int fallRecoveryResetCounter = 0;
 
-// 快速下坠检测相关参数
-bool is_falling = false;         // 快速下坠状态标志
+// 快速下坠状态标志
 
 // 开机默认坐下 修改
 bool robot_enabled = false; // 开机禁用平衡，默认坐下
@@ -232,6 +246,9 @@ Wrobot wrobot;
 void setup()
 {
   Serial.begin(115200);   // 通讯串口
+  while (!Serial);     // 等待串口连接（USB串口需此逻辑）
+  Serial.println("参数调节就绪，格式：Kp=值;");
+
   Serial2.begin(1000000); // 腿部sts舵机
 
   // 舵机初始化
@@ -317,7 +334,8 @@ void setup()
 
 void loop()
 {
- 
+ serialReceiveProcess(); // 调用串口接收函数（核心修改）
+
   handleLEDBlink();       // 处理LED闪烁
   process_xbox_notif();   // 处理xbox手柄通知
   updateXboxVibration();  // 手柄震动状态
@@ -437,7 +455,7 @@ void loop()
       }else if(obstacle_level == "high"){ // 最高障碍，速度较慢
          trig_v_target = map(trigRT, 0, 1023, -2.0f, -8.0f);
       }else{ // 中度障碍，加个系数
-        trig_v_target = map(trigRT, 0, 1023, -2.0f, -8.0f) * 1.1f;
+        trig_v_target = map(trigRT, 0, 1023, -2.0f, -8.0f) * 1.2f;
       }
 
       motor1.target = motor2.target = trig_v_target; 
@@ -447,6 +465,10 @@ void loop()
 
   // 小车坐下
   if (sitting_down) {
+
+    // 关闭超级平衡模式
+    cancel_super_mode();
+
     sitting_down ++;
 
     if (sitting_down < 15) {
@@ -546,6 +568,31 @@ void lqr_balance_loop()
     speed_target_coeff = 0.1;
   }
  
+  // 超级平衡模式
+  if (super_balance_mode && !is_falling) {
+   
+    // if(abs(LQR_angle) < 4.0f ){
+    //    pid_super_balance.error_prev = 0.0f; // 重置积分项
+    // }
+
+     // 误差=实际角度 - 默认零点
+    float balance_joyy = pid_super_balance(LQR_angle -pitch_zeropoint);
+    balance_joyy = constrain(balance_joyy, -100.0f, 100.0f); 
+    wrobot.joyy = balance_joyy;
+    
+    // Serial.print("LQR_angle:");
+    // Serial.print(LQR_angle);
+    // Serial.print(",balance_joyy:");
+    // Serial.println(balance_joyy);
+  }
+  // else{
+  //   //  Serial.print("LQR_angle:");
+  //   //  Serial.print(LQR_angle);
+  //     Serial.print(",wrobot.joyy:");
+  //     Serial.println(wrobot.joyy);
+  // }
+
+
   speed_control = pid_speed(LQR_speed - speed_target_coeff * lpf_joyy(wrobot.joyy));  // 最大8v
 
   // 检测轮子差速，判断轮子是否离地
@@ -648,25 +695,31 @@ void leg_loop()
   // 1. 获取并处理MPU6050 Z轴加速度
   float z_accel = mpu6050.getAccZ(); // 1.1~1.2左右
 
-  // 2. 触发重力感应
-  if (z_accel >1.8f && !is_falling 
+  // Serial.print("z_accel:");
+  // Serial.println(z_accel);
+
+  // 2 触发重力感应-向下
+  if (z_accel > zaccel_threshold  && !is_falling 
     && stand_up_count == 0
     && roll_adjust == original_roll_adjust
     && leg_height_base == original_leg_height_base
     && !obstacle_btn_down
     && !obstacle_flag_count 
+    && (millis() - last_falling_trigger_time > FALLING_COOLDOWN) // 核心：5秒冷却
   ) {
     is_falling = true;
+    last_falling_trigger_time = millis(); // 更新触发时间戳
     // 腿部收缩
     ACC[0] = ACC[1] = 250;
     Speed[0] = Speed[1] = 1000;
     Position[0] = SERVO0_MIN + 150;
     Position[1] = SERVO1_MIN - 150;
     sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
+    resetZeroPoint();
     return; // 跳过后续正常高度计算
   }
 
-  // // 3. 下坠结束判断与恢复逻辑
+  //  下坠重力感应后舵机结束判断与恢复逻辑
   if (is_falling) {
     if (!sms_sts.ReadMove(ID[0]) && !sms_sts.ReadMove(ID[1])) {
       is_falling = false;
@@ -881,6 +934,7 @@ void yaw_loop()
 void resetZeroPoint() {
   distance_zeropoint =  LQR_distance;
   pid_lqr_u.error_prev = 0;
+  pid_super_balance.error_prev = 0.0f; // 重置积分项
 }
 
 /*========================================
@@ -1124,6 +1178,20 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
   }
   last_btnShare = remoteData.btnShare; // 更新btnY的上一次状态
 
+  // 超级平衡模式切换
+if (remoteData.btnSelect && !last_btnSelect) {
+    super_balance_mode = !super_balance_mode;
+    if (super_balance_mode) {
+      setLEDColor(CRGB::Blue);
+      zaccel_threshold = super_zaccel_threshold;
+      resetZeroPoint(); // 重置位移零点，确保初始平衡
+    } else {
+      cancel_super_mode();
+    }
+}
+last_btnSelect = remoteData.btnSelect;
+
+
   // 舵机中位校准 中间十字左右键同时按下
   if (calibrationOfs_flag == 0 && remoteData.btnSelect && last_btnSelect && remoteData.btnStart && last_btnStart) { 
       calibrationOfs_flag =1;
@@ -1330,3 +1398,12 @@ void obstacle_loop()
     startLEDBlink(CRGB::Green, 100,  2 );
   }
 }
+
+
+void cancel_super_mode(){
+  super_balance_mode = false;
+  zaccel_threshold = normal_zaccel_threshold;
+  setLEDColor(CRGB::Black);
+  resetZeroPoint(); 
+  wrobot.joyy = 0; // 清空补偿，快速回稳
+}