From 3d65b2f1ce16537d003d5091007fb0a6841e5809 Mon Sep 17 00:00:00 2001 From: myperworld Date: Sun, 23 Nov 2025 21:44:44 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E4=B8=80=E4=B8=AA=E7=AE=80?= =?UTF-8?q?=E5=8D=95=E7=9A=84=E8=B6=8A=E9=9A=9C=E6=A8=A1=E5=BC=8F=E5=92=8C?= =?UTF-8?q?=E6=96=9C=E5=9D=A1=E8=87=AA=E5=B9=B3=E8=A1=A1=E5=8A=9F=E8=83=BD?= =?UTF-8?q?=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 2 +- v1.3/src/SerialParser.cpp | 48 ++++++++++++++++++++ v1.3/src/SerialParser.h | 21 +++++++++ v1.3/src/main.cpp | 95 +++++++++++++++++++++++++++++++++++---- 4 files changed, 156 insertions(+), 10 deletions(-) create mode 100644 v1.3/src/SerialParser.cpp create mode 100644 v1.3/src/SerialParser.h diff --git a/README.md b/README.md index 63d723c..1cbef45 100644 --- a/README.md +++ b/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/。 \ No newline at end of file +2025-11-23 v1.3增加一个简单的越障模式，和斜坡自平衡功能. \ No newline at end of file diff --git a/v1.3/src/SerialParser.cpp b/v1.3/src/SerialParser.cpp new file mode 100644 index 0000000..a2db745 --- /dev/null +++ b/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); +} \ No newline at end of file diff --git a/v1.3/src/SerialParser.h b/v1.3/src/SerialParser.h new file mode 100644 index 0000000..191d672 --- /dev/null +++ b/v1.3/src/SerialParser.h @@ -0,0 +1,21 @@ +#ifndef SERIAL_PARSER_H +#define SERIAL_PARSER_H + +#include +#include // 包含 PIDController 类的定义 +#include // 包含 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 \ No newline at end of file diff --git a/v1.3/src/main.cpp b/v1.3/src/main.cpp index d08bbcd..f2832a6 100644 --- a/v1.3/src/main.cpp +++ b/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; // 清空补偿，快速回稳 +} \ No newline at end of file