diff --git a/v1.3/src/SerialParser.cpp b/v1.3/src/SerialParser.cpp index 3fbd6ac..4a7eb4b 100644 --- a/v1.3/src/SerialParser.cpp +++ b/v1.3/src/SerialParser.cpp @@ -30,11 +30,11 @@ void parseSingleParam(char* cmd) { float param_val = atof(eq_pos + 1); // 转为浮点值 // 匹配参数名并赋值（访问全局对象，已在 SerialParser.h 中声明） - if (strcmp(param_name, "Kp") == 0) { + if (strcmp(param_name, "kp") == 0) { pid_super_balance.P = param_val; - } else if (strcmp(param_name, "Ki") == 0) { + } else if (strcmp(param_name, "ki") == 0) { pid_super_balance.I = param_val; - } else if (strcmp(param_name, "Kd") == 0) { + } 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; diff --git a/v1.3/src/SerialParser.h b/v1.3/src/SerialParser.h index 235233c..0dd3ab0 100644 --- a/v1.3/src/SerialParser.h +++ b/v1.3/src/SerialParser.h @@ -8,6 +8,7 @@ // -------------------------- 全局对象声明（与 main.cpp 中定义一致）-------------------------- // 这些对象在 main.cpp 中定义，此处用 extern 声明供 SerialParser 访问 extern PIDController pid_super_balance; +extern PIDController pid_speed; extern float zaccel_threshold; diff --git a/v1.3/src/main.cpp b/v1.3/src/main.cpp index 4e0a487..6af022b 100644 --- a/v1.3/src/main.cpp +++ b/v1.3/src/main.cpp @@ -159,7 +159,7 @@ float gyro_control = 0; float speed_control = 0; float distance_control = 0; float LQR_u = 0; -float pitch_zeropoint = 2; // 7 设定默认俯仰角度，向前到正，向后倒负 +float pitch_zeropoint = 2; // 2 设定默认俯仰角度，向前到正，向后倒负 float original_pitch_zeropoint = pitch_zeropoint; // 保存原始的角度零点 float distance_zeropoint = 0.5f; // 轮部位移零点偏置 float pitch_adjust = 0.0f; // 俯仰角度调整,负数前倾，正数后倾 @@ -182,6 +182,7 @@ float robot_speed_diff = 0; // 速度差 int jump_flag = 0; // 跳跃过程计数 int jump_pre_flag = 0; // 跳跃前蓄能计数 +int place_jump_flag = 0; // 原地跳跃 int forward_jump_flag = 0; // 前跳跃标志位 int back_jump_flag = 0; // 后跳跃标志位 int left_jump_flag = 0; // 左跳跃标志位 @@ -223,7 +224,7 @@ float high_motor_target = -9.0f; // 超级平衡模式参数 bool super_balance_mode = false; // 超级平衡模式 -PIDController pid_super_balance(11, 0, 0, 100000, 100); // 适配joyy实际需求 +PIDController pid_super_balance(11.5, 0, 0, 100000, 8); // 适配joyy实际需求 // 舵机中位校准 int calibrationOfs_flag = 0; @@ -248,6 +249,9 @@ unsigned long pauseStartTime = 0; int currentStepIndex = 0; RobotAction* currentAction = NULL; +// 龙头模式 +bool dragon_head_mode = false; // 龙头模式标志 + typedef struct { @@ -438,6 +442,12 @@ void loop() motor2.target = -0.8; } + // 原地跳越 + if ( place_jump_flag > 2) { + motor1.target = 1.5; // 脉冲 + motor2.target = 1.5; + } + // 站立计数 if( stand_up_count > 0){ stand_up_count ++; @@ -572,13 +582,11 @@ void lqr_balance_loop() speed_target_coeff = 0.1; } - // 超级平衡模式 - if (super_balance_mode && !is_falling && robot_enabled && !sitting_down && !jump_flag) { - // 误差=实际角度 - 默认零点 + // 超级平衡模式 误差=实际角度 - 默认零点 + if (super_balance_mode && !is_falling && robot_enabled && !sitting_down && !jump_flag && !dragon_head_mode) { float balance_joyy = pid_super_balance(LQR_angle -pitch_zeropoint); balance_joyy = constrain(balance_joyy, -100.0f, 100.0f); wrobot.joyy = balance_joyy; - } speed_control = pid_speed(LQR_speed - speed_target_coeff * lpf_joyy(wrobot.joyy)); // 最大8v @@ -657,8 +665,7 @@ void lqr_balance_loop() pid_lqr_u.error_prev = 0; // 输出积分清零 } - // 平衡控制参数自适应 - // 考虑 leg_height_base = 0 + if (wrobot.height < 50){ pid_speed.P = 0.7; }else if (wrobot.height < 64){ @@ -666,6 +673,7 @@ void lqr_balance_loop() }else{ pid_speed.P = 0.5; } + } // 腿部动作控制（舵机） @@ -674,7 +682,7 @@ void leg_loop() // 如果坐下就不控制腿部 if(sitting_down || jump_flag || jump_pre_flag - || left_jump_flag || right_jump_flag || forward_jump_flag || back_jump_flag + || left_jump_flag || right_jump_flag || forward_jump_flag || back_jump_flag || place_jump_flag || obstacle_enconter_count ){ return; // 跳过后续leg高度计算 @@ -808,6 +816,7 @@ void jump_loop() left_jump_flag = 0; right_jump_flag = 0; forward_jump_flag = 0; + place_jump_flag = 0; back_jump_flag = 0; startLEDBlink(CRGB::Yellow, 100, 2 ); return; // 跳过后续leg高度计算 @@ -847,6 +856,14 @@ void jump_loop() if(back_jump_flag > 15){ back_jump_flag = 0; } + + }else if( place_jump_flag ){ // 原地跳跃 + place_jump_flag ++ ; + + if(place_jump_flag > 20){ + place_jump_flag = 0; + } + }else{ // 跳跃开始 @@ -876,16 +893,16 @@ void jump_loop() ACC[1] = 0; Speed[0] = 0; Speed[1] = 0; - Position[0] = (SERVO0_MIN + 100); // 100 约等于食指宽度 - Position[1] = (SERVO1_MIN - 100); + Position[0] = (SERVO0_MIN + 80); // 100 约等于食指宽度 + Position[1] = (SERVO1_MIN - 80); sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC); } // 轮子着地 - if (jump_flag > 200) // 2000=6s ,1s=2000/6=333.33 + // if (jump_flag > 200) // 2000=6s ,1s=2000/6=333.33 + if (jump_flag > 150) // 2000=6s ,1s=2000/6=333.33 { jump_flag = 0; // 跳跃过程结束 - resetZeroPoint(); } } @@ -900,15 +917,15 @@ void yaw_loop() return; } - // 1. 根据abs(wrobot.joyx)，动态设置yaw_target系数 + // 1. 根据abs(wrobot.joyx)，动态设置yaw_target系数0.1~1 float coeff = map( abs(wrobot.joyx), 0, 100, 10, 100) / 100.0f; coeff = constrain(coeff, 0.1f, 1.0f); float yaw_target = wrobot.joyx * coeff; - // 2. 根据abs(wrobot.joyx)，提高PID响应：临时放大yaw_angle_control（核心突破上限） - float yaw_angle_control_coeff = 1.0f; - if( wrobot.joyy == 0 && abs(wrobot.joyx) > 10){ // 原地打转 - yaw_angle_control_coeff = map( abs(wrobot.joyx), 0, 100, 1.0, 2.0); + // 2. 根据abs(wrobot.joyx)，提高PID响应，临时放大系数 + float yaw_angle_fator = 1.0f; + if( wrobot.joyy == 0 && abs(wrobot.joyx) > 10 && !dragon_head_mode){ // 原地打转 + yaw_angle_fator = map( abs(wrobot.joyx), 0, 100, 1.0, 2.0); // 根据转向速度，智能计算wrobot.height // abs(wrobot.joyx) 范围0到100，默认0，输出范围20-50,默认 30 // 反向映射：绝对值30→60，绝对值100→0（线性过渡） @@ -917,18 +934,24 @@ void yaw_loop() wrobot.height = constrain(wrobot.height, 30, 50); // 使用低波过滤器，平滑数据 wrobot.height = lpf_height(wrobot.height); - + // 增加rgb效果 startLEDBlink(CRGB::Red, 200, 1); } - float yaw_angle_control = pid_yaw_angle(yaw_target) * yaw_angle_control_coeff; + float yaw_angle_control = pid_yaw_angle(yaw_target) * yaw_angle_fator; + // 限制yaw_angle_control上限，避免超压（按电机7.4V反推，设为12较合适） yaw_angle_control = constrain(yaw_angle_control, -12.0f, 12.0f); + YAW_gyro = (float)mpu6050.getGyroZ(); // 左右偏航角速度，用于纠正小车前后走直线时的角度偏差 float yaw_gyro_control = pid_yaw_gyro(YAW_gyro); + YAW_output = yaw_angle_control + yaw_gyro_control; + + + } // 重置距离零点 @@ -936,11 +959,11 @@ void resetZeroPoint() { distance_zeropoint = LQR_distance; pid_lqr_u.error_prev = 0; pid_super_balance.error_prev = 0.0f; // 重置积分项 - pitch_adjust = 0.0f; + pitch_adjust = 0.0f; } /*======================================== - * 控制器数据处理（共用函数） + * xbox 手柄控制 * ========================================*/ void processControllerData(const XboxControllerNotificationParser& remoteData) { @@ -1020,19 +1043,43 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) { wrobot.joyy = 0; } - // 左右转向 左摇杆X轴：先映射到-100~100，再按比例放大到-100~100 - int convertedJ1PotX = constrain( - map(remoteData.joyLHori, 0, 65535, -100, 100), - -100, 100 - ); - if(abs(convertedJ1PotX) > deadZone) { // 左右运动 -100 到 100 - wrobot.joyx = convertedJ1PotX; - } else { - wrobot.joyx = 0; + // 龙头模式 + if (remoteData.btnStart && !last_btnStart) { + dragon_head_mode = !dragon_head_mode; // 切换模式 + if (dragon_head_mode) { + setLEDColor(CRGB::Purple); // 紫色LED标识龙头模式 + } else { + setLEDColor(CRGB::Black); // 关闭LED + } } + // 左右转向 左摇杆X轴：先映射到-100~100，再按比例放大到-100~100 + if(!dragon_head_mode){ // 龙头模式 + int convertedJ1PotX = constrain( + map(remoteData.joyLHori, 0, 65535, -100, 100), + -100, 100 + ); + if(abs(convertedJ1PotX) > deadZone) { // 左右运动 -100 到 100 + wrobot.joyx = convertedJ1PotX; + } else { + wrobot.joyx = 0; + } + + }else{ + int convertedJ2PotX = constrain( + map(remoteData.joyRHori, 0, 65535, -100, 100), + -100, 100 + ); + if(abs(convertedJ2PotX) > deadZone) { // 左右运动 -100 到 100 + wrobot.joyx = convertedJ2PotX * 0.3; + } else { + wrobot.joyx = 0; + } + } + // 智能计算wrobot.height ，根据前后速度， // convertedJ2PotY 范围-100到100，默认0，输出范围30（最矮）-50（最高）, 默认 50 + // wrobot.height 0 最矮，50最高 wrobot.height = map( abs(convertedJ2PotY), 0, 100, 50, 30); wrobot.height = constrain(wrobot.height, 30, 50); wrobot.height = lpf_height(wrobot.height); @@ -1113,6 +1160,7 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) { // 跳跃 右摇杆按键 if (remoteData.btnRS && !last_btnRS && robot_enabled) { // 1=true，0=false wrobot.jump = 1; // 跳跃 + place_jump_flag = 1; // 跳跃 }else{ wrobot.jump = 0; // 不跳跃 } @@ -1179,17 +1227,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 (remoteData.btnSelect && !last_btnSelect) { + + pid_super_balance.error_prev = 0.0f; + + 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; // 舵机中位校准 中间十字左右键同时按下 @@ -1206,6 +1257,7 @@ last_btnSelect = remoteData.btnSelect; if(calibrationOfs_flag > 500){ calibrationOfs_flag =0; } + last_btnSelect = remoteData.btnSelect; last_btnStart = remoteData.btnStart; } @@ -1296,9 +1348,6 @@ void cancel_super_mode(){ // 障碍检测 void obstacle_detect() { - // Serial.print("LQR_speed"); - // Serial.println(LQR_speed); - // 自动越障前进模式下,持续300次循环，防止刚启动由于惯性无法立即向前 if(!obstacle_btn_down){ return;