diff --git a/v1.4 摄像头提乒乓球方案/platformio/src/SerialParser.cpp b/v1.4 摄像头提乒乓球方案/platformio/src/SerialParser.cpp index e00d2fb..56ef08d 100644 --- a/v1.4 摄像头提乒乓球方案/platformio/src/SerialParser.cpp +++ b/v1.4 摄像头提乒乓球方案/platformio/src/SerialParser.cpp @@ -131,6 +131,11 @@ void parseSingleParam(char* cmd) { pid_trace_joyy.P = param_val; } else if (strcmp(param_name, "pid_trace_joyy_d") == 0) { pid_trace_joyy.D = param_val; + + } else if (strcmp(param_name, "pid_yaw_align_p") == 0) { + pid_yaw_align.P = param_val; + } else if (strcmp(param_name, "pid_yaw_align_d") == 0) { + pid_yaw_align.D = param_val; } else if (strcmp(param_name, "pid_cam_p") == 0) { @@ -149,6 +154,17 @@ void parseSingleParam(char* cmd) { run_kick_ball_dy = param_val; } else if (strcmp(param_name, "ob_ball_dy") == 0) { ob_ball_dy = param_val; + } else if (strcmp(param_name, "ready_kick_angel") == 0) { + ready_kick_angel = param_val; + } else if (strcmp(param_name, "ready_leg_height_base") == 0) { + ready_leg_height_base = param_val; + } else if (strcmp(param_name, "yaw_align_threshold") == 0) { + yaw_align_threshold = param_val; + + } else if (strcmp(param_name, "kick_back_s2") == 0) { + kick_back_s2 = param_val; + } else if (strcmp(param_name, "yaw_error_coff") == 0) { + yaw_error_coff = param_val; } else if (strcmp(param_name, "kick_over_count_max") == 0) { diff --git a/v1.4 摄像头提乒乓球方案/platformio/src/SerialParser.h b/v1.4 摄像头提乒乓球方案/platformio/src/SerialParser.h index b2f5e6b..8f1bc17 100644 --- a/v1.4 摄像头提乒乓球方案/platformio/src/SerialParser.h +++ b/v1.4 摄像头提乒乓球方案/platformio/src/SerialParser.h @@ -16,15 +16,22 @@ extern float dragon_joyx_factor; extern PIDController pid_trace_joyx; extern PIDController pid_trace_joyy; +extern PIDController pid_yaw_align; extern float pid_cam_p; extern float pid_cam_d; extern int chase_ball_s2; +extern int kick_back_s2; extern int ob_ball_dy; extern int place_ball_s2; extern int place_kick_ball_dy; extern int run_kick_ball_dy; extern int kick_over_count_max; +extern int yaw_align_threshold; + +extern int ready_kick_angel; +extern int ready_leg_height_base; +extern float yaw_error_coff; extern float leg_height_base; @@ -67,22 +74,21 @@ void kick_ball_loop(); void SetKickMode(String mode); void kick_ball_main(int dx, int dy); -// void chase_ball(int dy); -void chase_and_kick_ball(int dy); -void place_kick_ball(int dy); void kick_ball_core(); void ready_kick_ball(); -void aim_ob ( int dx, int dy, String ob); -void aim_ball(int dx, int dy); -void aim_face ( int dx, int dy); +void chase_ball(int dy); +void align_yaw_zero(); + +void joyx_aim_ball(int dx); +void cam_aim_ball(int dy); void setCarmeraFixServoAngles(int servo_angle); void setFrontierServoAngles(int servo_angle); void setServoAngles(int s1, int s2); -void cancel_trace_mode(); +void cancel_kick_mode(); void SerialToMaixcam(String cat, String str); diff --git a/v1.4 摄像头提乒乓球方案/platformio/src/main.cpp b/v1.4 摄像头提乒乓球方案/platformio/src/main.cpp index d3a4359..b9f8568 100644 --- a/v1.4 摄像头提乒乓球方案/platformio/src/main.cpp +++ b/v1.4 摄像头提乒乓球方案/platformio/src/main.cpp @@ -277,7 +277,6 @@ Servo cam_fix_servo; // 追踪模式 bool trace_mode = false; // 追踪模式整体标志 String kick_mode = ""; // 原地踢球模式kick_place_mode 追逐踢球模式kick_run_mode -String kick_status = ""; // aim_ball aim_face aim_ball_over aim_face_over kick_ball_over bool trace_ob_flag = false; // 是否追踪到位 int dx_to_joyx = 9999; @@ -293,10 +292,14 @@ int place_kick_ball_dy = -10; // 追逐球，并踢球 int chase_ball_s2 = 30; // 20 -int run_kick_ball_dy = -5; // 20 +int kick_back_s2 = 40; // 20 +int run_kick_ball_dy = -5; int ob_ball_dy = 120; int kick_over_count = 0; // 运行踢球后计数 -int kick_over_count_max = 150; // 运行踢球后计数最大值 +int kick_over_count_max = 50; // 运行踢球后计数最大值 +float yaw_error_coff = 1.0f; // 偏航角误差系数 +int ready_kick_angel = 100; // 准备踢球角度 +int ready_leg_height_base = 22.0f; // 准备踢球高度 // 摄像头舵机 int current_s1 = 0; // 前档舵机实时角度 @@ -309,6 +312,13 @@ int pid_trace_joyy_output_limit = 10; // PD输出限幅（单次最大调整角 int last_dy = 0; // 上一次的y轴偏差 unsigned long last_dy_time = 0; // 上一次计算偏差的时间戳 +// 踢球模式偏航角校准相关 +float kick_yaw_zero = 0.0f; // kick_run_mode下的偏航基准角（0度） +int yaw_align_threshold = 10; // 偏航角对齐阈值 +bool chased_flag = false; // 已经追到球了 +bool align_flag = false; // 已经对齐偏航角了 +int yaw_to_joyx = 9999; // 偏航对齐专用的joyx控制值（9999表示无效） +PIDController pid_yaw_align(0.8, 0, 0.2, 100000, 100); // 偏航对齐专用PID void setup() { @@ -628,8 +638,8 @@ void lqr_balance_loop() gyro_control = pid_gyro(LQR_gyro); - - if(kick_mode == "kick_run_mode"){ + // 踢球模式 + if(trace_mode){ if(dy_to_joyy!= 9999){ wrobot.joyy = dy_to_joyy; }else{ @@ -992,6 +1002,8 @@ void yaw_loop() if(trace_mode){ if(dx_to_joyx!= 9999){ wrobot.joyx = dx_to_joyx; + }else if(yaw_to_joyx != 9999){ + wrobot.joyx = yaw_to_joyx; }else{ wrobot.joyx = 0; } @@ -1081,6 +1093,8 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) { sitting_down = 1; // 标记开始坐下动作保存当前的角度零点 pid_trace_joyx.P = 0.5; // 追踪模式 + + cancel_kick_mode(); } // 第二次按坐下，让舵机失能 if (remoteData.btnLB && !last_btnLB && !robot_enabled) { @@ -1279,6 +1293,14 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) { wrobot.jump = 1; forward_jump_flag = 1; } + // 追逐并转向踢会球模式 @@@@@@ + if (remoteData.btnY && !last_btnY && robot_enabled && trace_mode) { + if(kick_mode != "kick_back_mode"){ + SetKickMode("kick_back_mode"); + }else if(kick_mode == "kick_back_mode"){ + SetKickMode("kick_place_mode"); + } + } last_btnY = remoteData.btnY; // 向后跳跃（非踢球模式） @@ -1288,7 +1310,7 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) { } // 追逐踢球模式 @@@@@@ if (remoteData.btnA && !last_btnA && robot_enabled && trace_mode) { - if(kick_mode == "kick_place_mode"){ + if(kick_mode != "kick_run_mode"){ SetKickMode("kick_run_mode"); }else if(kick_mode == "kick_run_mode"){ SetKickMode("kick_place_mode"); @@ -1323,7 +1345,7 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) { if(trace_mode){ SetKickMode("kick_place_mode"); // 原地踢球模式 }else { - cancel_trace_mode(); + cancel_kick_mode(); } } @@ -1548,34 +1570,36 @@ void kick_ball_loop(){ if(trace_mode){ if (kick_mode == "kick_run_mode") { // 运动追踪模式 setLEDColor(CRGB::Blue); - }else { + }else if(kick_mode == "kick_back_mode") { // 追逐并转向踢会球模式 + setLEDColor(CRGB::Red); + }else if(kick_mode == "kick_place_mode"){ // 原地踢球模式 setLEDColor(CRGB::Green); } - } - - // 动态调整高度 - if(wrobot.joyy == 0 ){ - if(current_s1 > 90){ - leg_height_base = 30.0f; - }else if(current_s1 > 10){ - leg_height_base = constrain(current_s1, 15.0f, 30.0f); - }else{ - leg_height_base = original_leg_height_base; + }else{ + // 非踢球模式下，动态调整高度 + if(wrobot.joyy == 0 ){ + if(current_s1 > 90){ // 打开挡板 + leg_height_base = 30.0f; + }else if(current_s1 > 10){ + leg_height_base = constrain(current_s1, 15.0f, 30.0f); + }else{ + leg_height_base = original_leg_height_base; + } } } - - // 在运动踢球时，踢好球，需要有一段停止前进时间 - if(kick_over_count > 0 && kick_over_count < kick_over_count_max){ - kick_over_count ++; - }else{ - kick_over_count = 0; - } - } // 通过偏差deviation 控制摄像头上下左右运动 void kick_ball_main(int dx, int dy) { + // 在运动踢球时，踢好球，需要有一段停止前进时间 + // if(kick_over_count > 0 && kick_over_count < kick_over_count_max){ + // kick_over_count ++; + // }else{ + // kick_over_count = 0; + // } + // Serial.println("kick_over_count: " + String(kick_over_count) ); + // 不是追踪模式 if(!trace_mode){ dx_to_joyx = 9999; // 不左右旋转 @@ -1587,72 +1611,110 @@ void kick_ball_main(int dx, int dy) { return; } // 追踪模式，找不到目标, 略微低头 - if( trace_mode && (dx == 9999 or dy == 9999) ){ + if( trace_mode && (dx == 9999 or dy == 9999)){ dx_to_joyx = 9999; // 不左右旋转 dy_to_joyy = 9999; // 不前后追逐球 + trace_ob_flag = false; last_dy = 0; // 重置相机舵机PID控制器状态 last_dy_time = 0; - setCarmeraFixServoAngles(60); - - setFrontierServoAngles(0); - - return; + setCarmeraFixServoAngles(60); // 摄像头低头 + setFrontierServoAngles(0); // 关闭挡板 + // return; } // Serial.println("s2: " + String(current_s2) + " dy: " + String(dy)); // 原地踢球模式 if( kick_mode == "kick_place_mode"){ - aim_ball(dx,dy); - place_kick_ball(dy); + cam_aim_ball(dy); // 相机瞄准 + joyx_aim_ball(dx); // 控制左右 使小车正面是否朝向物体 + if(current_s2 < place_ball_s2 && dy > place_kick_ball_dy && dy < 120){ // 球到车身下方了，打球 + kick_ball_core(); + }else { // 追踪到目标 + ready_kick_ball(); // 准备踢球 + } } // 运动踢球模式 if(kick_mode == "kick_run_mode"){ - - aim_ball(dx,dy); - - if(kick_over_count){ // 踢好球之后阶段，后退 - dy_to_joyy = -50; + cam_aim_ball(dy); // 相机瞄准 + + if(kick_over_count){ // 踢好球之后阶段 + + if(kick_over_count > 0 && kick_over_count < kick_over_count_max){ + kick_over_count ++; + }else{ + kick_over_count = 0; + yaw_to_joyx = 9999; // 不旋转偏航角 + } + + if(!align_flag){ + align_yaw_zero(); // 对齐偏航角 + }else{ + dy_to_joyy = -50; // 后退 + } + }else{ - chase_and_kick_ball(dy); + align_flag = false; // 重置对齐标志 + joyx_aim_ball(dx); // 控制左右 使小车正面是否朝向物体 + + if(chased_flag){ + if(kick_over_count == 0){ + dy_to_joyy = 9999; + kick_ball_core(); // 踢球 + kick_over_count = 1; // 计数用于后退 + chased_flag = false; + } + }else{ + chase_ball(dy); + } } - } } -// 追逐球，并踢球 -void chase_and_kick_ball(int dy){ - // 物体在中间 dy=0 - // 物体在上方 dy趋近-100 - // 物体在下方 dy趋近 100 - // 已经追到球了 - if(current_s2 < chase_ball_s2 && dy > run_kick_ball_dy && dy < 120){ +void align_yaw_zero(){ - dy_to_joyy = 9999; - kick_ball_core(); // 踢球 - kick_over_count = 1; // 计数用于后退 - - }else{ // 通过pid 向前运动 + // 获取当前偏航角 & 计算与基准角的偏差 + // 小车正对人，kick_yaw_zero 为基准角 + // 向右（以人为标准），current_yaw变大（变正数），yaw_error 变小负数 + // 向左（以人为标准），current_yaw变小（变负数），yaw_error 变大正数 + float current_yaw = mpu6050.getAngleZ(); + float yaw_error = kick_yaw_zero - current_yaw; // 基准角 - 当前角 = 需修正的偏差 + + // 串口测试，写在一行 + // Serial.println( "Zero:" + String(kick_yaw_zero) + ",Current:" + String(current_yaw) + ",Error:" + String(yaw_error)); - ob_y = ob_ball_dy - dy; - dy_to_joyy = pid_trace_joyy(ob_y); + if(abs(yaw_error) < yaw_align_threshold){ + Serial.println("aligned"); + // 偏航角已对齐：更新状态为“已对齐” + yaw_to_joyx = 9999; // 重置偏航对齐控制值 + align_flag = true; // 已经对齐偏航角了 + pid_yaw_align.error_prev = 0.0f; // 重置偏航对齐控制器状态 - ready_kick_ball(); // 准备踢球 - - } + } else { + // Serial.println("aligning"); + // 未对齐，通过专用PID计算yaw_to_joyx控制转向 + int joyx_direction = (yaw_error > 0) ? -1 : 1; // 偏差为正：需要右转；偏差为负：需要左转 + yaw_to_joyx = constrain(pid_yaw_align(abs(yaw_error * yaw_error_coff)) * joyx_direction, -100, 100); + // dy_to_joyy = 9999; // 对齐期间停止前进 + align_flag = false; + } } - -// 原地踢球模式的踢球 -void place_kick_ball(int dy){ - - if(current_s2 < place_ball_s2 && dy > place_kick_ball_dy && dy < 120){ // 球到车身下方了，打球 - kick_ball_core(); - }else { // 追踪到目标 +// 追逐球，并转向，踢回球 +void chase_ball(int dy){ + // 物体在中间 dy=0, 物体在上方 dy趋近-100, 物体在下方 dy趋近 100 + // 已经追到球了 + if(current_s2 < chase_ball_s2 && dy > run_kick_ball_dy && dy < 120){ + chased_flag = true; + // Serial.println("chased"); + }else{ // 未追到球，继续追逐 + ob_y = ob_ball_dy - dy; + dy_to_joyy = pid_trace_joyy(ob_y); ready_kick_ball(); // 准备踢球 + // Serial.println("chasing"); } } @@ -1663,37 +1725,17 @@ void kick_ball_core(){ } // 准备踢球，打开挡板，收到后方 void ready_kick_ball(){ - setFrontierServoAngles(100); // 打开挡板，收到后方 - leg_height_base = 30.0f; // 下降 + if( abs(current_s1 - ready_kick_angel) > 5){ + setFrontierServoAngles(ready_kick_angel); // 打开挡板，收到后方 + leg_height_base = ready_leg_height_base; // 下降 + } } - -void aim_ob ( int dx, int dy, String ob){ - //----- 控制左右 使小车正面是否朝向物体-------- - // 物体在左方 dx趋近-150 - // 物体在右方 dx趋近 150 - // 中间 dx=0 只需要一个区间 50 - // 判断是否瞄准 - if( abs(dx) < 50){ // 小车正面已经瞄准 - - trace_ob_flag = true; - dx_to_joyx = 9999; // 已经追踪到了目标，不需要再左右调整旋转 - pitch_adjust = 0.0f; // debug - - }else{ // 没有瞄准 - trace_ob_flag = false; +// 相机瞄准 +void cam_aim_ball (int dy){ + if(dy == 9999){ + return; } - - // pid 左右旋转 瞄准目标 - if(!trace_ob_flag){ - - int joyx_derection = (dx > 0) ? -1 : 1; - dx_to_joyx = pid_trace_joyx(abs(dx)) * joyx_derection; - - }else{ // 已经正面朝向物体 - } - - //----- 控制上下 相机舵机控制----------- // 物体在上方 dy趋近-100 // 物体在下方 dy趋近 100 // 中间 dy = 0 @@ -1724,13 +1766,33 @@ void aim_ob ( int dx, int dy, String ob){ // 偏差过小，重置PD状态 last_dy = 0; } +} -} -void aim_ball ( int dx, int dy){ - aim_ob ( dx, dy, "ball"); -} -void aim_face ( int dx, int dy){ - aim_ob ( dx, dy, "face"); +// 控制左右 使小车正面是否朝向物体 +void joyx_aim_ball (int dx){ + if(dx == 9999){ + return; + } + //----- 控制左右 使小车正面是否朝向物体-------- + // 物体在左方 dx趋近-150 + // 物体在右方 dx趋近 150 + // 中间 dx=0 只需要一个区间 50 + // 判断是否瞄准 + if( abs(dx) < 50){ // 小车正面已经瞄准 + + trace_ob_flag = true; + dx_to_joyx = 9999; // 已经追踪到了目标，不需要再左右调整旋转 + pitch_adjust = 0.0f; // debug + + }else{ // 没有瞄准 + trace_ob_flag = false; + } + + // pid 左右旋转 瞄准目标 + if(!trace_ob_flag){ + int joyx_derection = (dx > 0) ? -1 : 1; + dx_to_joyx = pid_trace_joyx(abs(dx)) * joyx_derection; + } } // 控制摄像头舵机 @@ -1759,17 +1821,21 @@ void SerialToMaixcam(String cat, String str){ } // 退出追踪模式 -void cancel_trace_mode(){ +void cancel_kick_mode(){ + + kick_over_count = 0; trace_mode = false; trace_ob_flag = false; + chased_flag = false; SetKickMode(""); - dx_to_joyx = 9999; dy_to_joyy = 9999; + yaw_to_joyx = 9999; pid_trace_joyx.error_prev = 0.0f; pid_trace_joyy.error_prev = 0.0f; + pid_yaw_align.error_prev = 0.0f; setLEDColor(CRGB::Black); leg_height_base = original_leg_height_base; // 上升 @@ -1777,5 +1843,14 @@ void cancel_trace_mode(){ } void SetKickMode(String mode){ + kick_mode = mode; + + yaw_to_joyx = 9999; + + // 进入kick_back_mode时，记录当前偏航角作为基准0度 + if(mode == "kick_run_mode" || mode == "kick_back_mode"){ + kick_yaw_zero = mpu6050.getAngleZ(); // 读取当前偏航角（Z轴角度）作为基准 + pid_yaw_align.error_prev = 0.0f; // 重置偏航对齐PID状态 + } } \ No newline at end of file