From 42fd44c28e91740690c64041934b0dcec66f559d Mon Sep 17 00:00:00 2001 From: myperworld Date: Thu, 30 Oct 2025 19:03:27 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E9=87=8D=E5=8A=9B=E6=84=9F?= =?UTF-8?q?=E5=BA=94=EF=BC=8C=E4=B8=BB=E6=89=93=E8=BA=AB=E6=AE=B5=E8=A6=81?= =?UTF-8?q?=E6=9F=94=E8=BD=AF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 4 +- v1.2/Shibo-main-TOMMY/src/main.cpp | 110 ++++++++++++++++++++++------- 2 files changed, 88 insertions(+), 26 deletions(-) diff --git a/README.md b/README.md index 1a7bb62..65cad6d 100644 --- a/README.md +++ b/README.md @@ -54,4 +54,6 @@ B站演示地址：https://space.bilibili.com/25336365 2025-10-17 v1.2版本优化xbox，盖世小鸡蓝牙手柄控制，增加动作集控制。 增加手柄震动功能，右扳机按下时，触发震动，连接时震动，断开时震动，起立时震动。 -2025-10-19 v1.2版本优化前后左右跳跃功能，前后长时间倒地后自恢复功能。 \ No newline at end of file +2025-10-19 v1.2版本优化前后左右跳跃功能，前后长时间倒地后自恢复功能。 + +2025-10-30 v1.2增加重力感应，主打身段要柔软。 \ No newline at end of file diff --git a/v1.2/Shibo-main-TOMMY/src/main.cpp b/v1.2/Shibo-main-TOMMY/src/main.cpp index 37aba32..6591653 100644 --- a/v1.2/Shibo-main-TOMMY/src/main.cpp +++ b/v1.2/Shibo-main-TOMMY/src/main.cpp @@ -15,12 +15,12 @@ Modify by Tommy 2025 /************函数申明*************/ - // 平衡算法函数 void lqr_balance_loop(); void yaw_loop(); void leg_loop(); void jump_loop(); +void obstacle_jump_loop(); void resetZeroPoint(); void jump_charge(); // 腿部弯曲（蓄能） @@ -28,6 +28,9 @@ void jump_charge(); // 腿部弯曲（蓄能） void processControllerData(const XboxControllerNotificationParser& data); void runActionSequence(); +// 障碍检测函数 +void detectObstacle(); + const int deadZone = 10; // 摇杆死区阈值 bool last_btnA = 0; bool last_btnB = 0; @@ -83,6 +86,7 @@ SMS_STS sms_sts; // MPU6050实例 MPU6050 mpu6050(I2Ctwo); + /************参数定义*************/ #define pi 3.1415927 @@ -112,6 +116,10 @@ s16 Position[2]; u16 Speed[2]; byte ACC[2]; +int last_servo_load = 0; // 上一时刻的舵机2负载 +unsigned long last_servo_sample_time = 0; // 上一次负载采样时间 +const unsigned long SERVO_SAMPLE_INTERVAL = 300; // 负载采样间隔（500毫秒） + // 腿部高度逻辑 // float leg_height_base 20 # 基准值 // float LegHeightDiff ; # 根据前后转向运动时，智能计算两腿高度差 @@ -137,6 +145,7 @@ float LQR_u = 0; float angle_zeropoint = 8; // 5 设定默认俯仰角度，向前到正，向后倒负 float original_angle_zeropoint = 8; // 保存原始的角度零点 float distance_zeropoint = 0.5f; // 轮部位移零点偏置 +// float pitch_adjust = 0.0f; // 俯仰角度调整 // YAW轴控制数据 float YAW_gyro = 0; @@ -152,18 +161,23 @@ const unsigned long SPEED_RECORD_INTERVAL = 100; // 转速记录间隔(毫秒) float last_lqr_speed = 0; // 记录上一时刻的轮部转速 float robot_speed_diff = 0; // 速度差 + int jump_flag = 0; // 跳跃过程计数 +int jump_pre_flag = 0; // 跳跃前蓄能计数 + int forward_jump_flag = 0; // 前跳跃标志位 int back_jump_flag = 0; // 后跳跃标志位 int left_jump_flag = 0; // 左跳跃标志位 int right_jump_flag = 0; // 右跳跃标志位 -int jump_pre_flag = 0; // 跳跃前蓄能计数 -int wheel_ground_flag = 1; // 轮子是否离地标记 默认离地 +int wheel_ground_flag = 1; // 轮子是否离地标记 默认离地 // 倒地恢复计数 int fallRecoveryResetCounter = 0; +// 快速下坠检测相关参数 +bool is_falling = false; // 快速下坠状态标志 + // 小车状态控制 // bool robot_enabled = true; // 小车使能状态，true为起立状态，false为坐下状态 // bool last_robot_enabled = true; // 记录上一次的小车状态 @@ -173,8 +187,9 @@ int fallRecoveryResetCounter = 0; bool robot_enabled = false; // 开机禁用平衡，默认坐下 bool last_robot_enabled = false; // 同步初始状态 int sitting_down = 1; // 开机直接进入“坐下状态”，避免无控制 +int stand_up_count = 0; // 站立标志位 -// 动作执行状态管理 +// 动作组执行状态管理 bool isExecutingAction = false; bool isPausing = false; unsigned long currentPauseDuration = 0; @@ -296,11 +311,14 @@ void loop() bat_check(); // 电压检测 delayMicroseconds(1500); mpu6050.update(); // IMU数据更新 + lqr_balance_loop(); // lqr自平衡控制 + yaw_loop(); // yaw轴转向控制 jump_loop(); // 跳跃动作 - leg_loop(); // 腿部动作控制 + leg_loop(); // 腿部动作控制 + runActionSequence(); // 执行动作序列 // 记录上一次的遥控数据数据 @@ -332,29 +350,30 @@ void loop() // 达到平衡角度后，重置重置计数器 if( abs(LQR_angle) < 25.0f){ fallRecoveryResetCounter = 0; + stand_up_count = 0; } - + // Serial.println(String(LQR_angle)); // 向左跳跃 - if ( left_jump_flag > 2 && left_jump_flag < 20) { + if ( left_jump_flag > 2) { motor1.target = 5.0; // 脉冲 motor2.target = -5.0; } // 向右跳跃 - if ( right_jump_flag > 2 && right_jump_flag < 20) { + if ( right_jump_flag > 2) { motor1.target = -5.0; // 脉冲 motor2.target = 5.0; } // 向前跳跃 - if ( forward_jump_flag > 2 && forward_jump_flag < 20) { + if ( forward_jump_flag > 2) { motor1.target = 2.3; // 脉冲 motor2.target = 2.3; } // 向后跳跃 - if ( back_jump_flag > 2 && back_jump_flag < 15) { + if ( back_jump_flag > 2 ) { motor1.target = -1.0; // 脉冲 motor2.target = -1.0; } @@ -442,9 +461,13 @@ void lqr_balance_loop() LQR_distance = (-0.5) * (motor1.shaft_angle + motor2.shaft_angle); // 两个电机的旋转角度（shaft_angle）,单位：弧度（rad）实际位移量 LQR_speed = (-0.5) * (motor1.shaft_velocity + motor2.shaft_velocity); // 两个电机角速度（shaft_velocity）,单位：弧度 / 秒（rad/s） LQR_angle = (float)mpu6050.getAngleY(); // mpu6050 pitch 角度，单位：度（°） + + // Serial.print("LQR_angle:"); + // Serial.println(LQR_angle); + LQR_gyro = (float)mpu6050.getGyroY(); // pitch Y轴角速度,单位：度 / 秒（°/s） - angle_control = pid_angle(LQR_angle - angle_zeropoint); + angle_control = pid_angle(LQR_angle - angle_zeropoint); // pitch_adjust gyro_control = pid_gyro(LQR_gyro); // 前进后退跳跃的系数都不一样 @@ -468,28 +491,27 @@ void lqr_balance_loop() if(robot_speed_diff > 18.0) // 轮子离地 { - wheel_ground_flag = 0; // 轮子离地标记 - // Serial.print("TAKE OFF"); + // wheel_ground_flag = 0; // 轮子离地标记 + // Serial.print("TAKE OFF"); // Serial.print(wheel_ground_flag); // Serial.print(",robot_speed_diff:"); - // Serial.print(robot_speed_diff); + // Serial.println(robot_speed_diff); // Serial.print(",jump_flag:"); // Serial.println(jump_flag); // 100 左右 } if( robot_speed_diff < -9.0) // 轮子着地 { - wheel_ground_flag = 1; // 轮子着地标记 + // wheel_ground_flag = 1; // 轮子着地标记 // Serial.print("LANDING:"); // Serial.print(",robot_speed_diff:"); - // Serial.print(robot_speed_diff); + // Serial.println(robot_speed_diff); // Serial.print(",jump_flag:"); // Serial.println(jump_flag); // 120左右 - if( jump_flag ){ // 落地点作为新的位移零点 resetZeroPoint(); } if( jump_flag == 0){ - XboxKeyVibration(); // 大减速运动状态手柄震动 + XboxKeyVibration(); // 大减速运动状态手柄震动 } } last_lqr_speed = LQR_speed; // 每隔100ms更新一次历史转速 @@ -550,12 +572,39 @@ void lqr_balance_loop() void leg_loop() { // 如果坐下就不控制腿部 - if(sitting_down || jump_flag || jump_pre_flag || left_jump_flag || right_jump_flag || forward_jump_flag || back_jump_flag){ + if(sitting_down + || jump_flag || jump_pre_flag + || left_jump_flag || right_jump_flag || forward_jump_flag || back_jump_flag){ return; // 跳过后续leg高度计算 } + // 1. 获取并处理MPU6050 Z轴加速度 + float z_accel = mpu6050.getAccZ(); // 1.1~1.2左右 - // 机身大角度，譬如倒地时，强制腿部收缩 + // 2. 快速下坠检测与处理 + if (z_accel >1.8f && !is_falling && stand_up_count == 0) { + is_falling = true; + // 腿部收缩 + ACC[0] = 250; + ACC[1] = 250; + Speed[0] = 1000; + Speed[1] = 1000; + Position[0] = SERVO0_MIN + 150; + Position[1] = SERVO1_MIN - 150; + sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); + return; // 跳过后续正常高度计算 + } + + // 3. 下坠结束判断与恢复逻辑 + if (is_falling) { + if (!sms_sts.ReadMove(ID[0]) && !sms_sts.ReadMove(ID[1])) { + is_falling = false; + }else{ + return; + } + } + + // 机身大角度倒地时，强制腿部收缩 if (abs(LQR_angle) > 25.0f) { // 默认舵机速度和加速度 @@ -569,6 +618,7 @@ void leg_loop() return; // 跳过后续leg高度计算 } + // 正常腿部高度计算 ACC[0] = wrobot.acc0; // 强转为byte（舵机库要求） ACC[1] = wrobot.acc1; Speed[0] = wrobot.speed0; // 强转为u16（舵机库要求） @@ -648,28 +698,28 @@ void jump_loop() if( left_jump_flag ){ left_jump_flag ++ ; - if( left_jump_flag >= 20 ){ + if( left_jump_flag > 20 ){ left_jump_flag = 0; } }else if( right_jump_flag ){ right_jump_flag ++ ; - if( right_jump_flag >= 20 ){ + if( right_jump_flag > 20 ){ right_jump_flag = 0; } }else if( forward_jump_flag ){ forward_jump_flag ++ ; - if(forward_jump_flag >= 15){ // 向前跳跃只有20个脉冲 + if(forward_jump_flag > 20){ // 向前跳跃只有20个脉冲 forward_jump_flag = 0; } }else if( back_jump_flag ){ back_jump_flag ++ ; - if(back_jump_flag >= 20){ + if(back_jump_flag > 15){ back_jump_flag = 0; } @@ -803,7 +853,6 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) { return; } - // 坐下 if (remoteData.btnLB && !last_btnLB && robot_enabled) { robot_enabled = false; @@ -820,6 +869,7 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) { if (remoteData.btnRB && !robot_enabled) { robot_enabled = true; sitting_down = 0; // 清除坐下动作标记 + stand_up_count ++; // 站立标志位 } last_btnRB = remoteData.btnRB; @@ -880,6 +930,7 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) { if(remoteData.btnDirUp) { leg_height_base -= 0.20; } + // 下降 if(remoteData.btnDirDown) { leg_height_base += 0.20; @@ -957,6 +1008,15 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) { XboxtrigRTVibration(); } + // 舵机中位校准 中间十字左右键同时按下 + if (remoteData.btnSelect && !last_btnSelect && remoteData.btnStart && !last_btnStart) { + sms_sts.CalibrationOfs(ID[0]); + sms_sts.CalibrationOfs(ID[1]); + XboxKeyVibration(100,1000); // 震动 + Serial.println("CalibrationOfs"); + } + last_btnSelect = remoteData.btnSelect; + last_btnStart = remoteData.btnStart; } // 执行连续动作的函数