更新踢球方案

v1.4 摄像头提乒乓球方案/platformio/src/SerialParser.cpp

@@ -132,6 +132,11 @@ void parseSingleParam(char* cmd) {
   } 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) {
     pid_cam_p = param_val;
@@ -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) {

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);
 

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,14 +1570,15 @@ 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); 
       }
-  }
+  }else{
-
+      // 非踢球模式下，动态调整高度
-  // 动态调整高度
       if(wrobot.joyy == 0 ){
-      if(current_s1 > 90){
+          if(current_s1 > 90){ // 打开挡板
             leg_height_base = 30.0f;
           }else if(current_s1 > 10){
             leg_height_base = constrain(current_s1, 15.0f, 30.0f);
@@ -1563,19 +1586,20 @@ void kick_ball_loop(){
             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; // 不左右旋转
@@ -1590,65 +1614,22 @@ void kick_ball_main(int dx, int dy) {
   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); 
+    setCarmeraFixServoAngles(60);  // 摄像头低头
-
+    setFrontierServoAngles(0);     // 关闭挡板
-    setFrontierServoAngles(0);
+    // return;
-
-    return;
   }
   
   // Serial.println("s2: " + String(current_s2) + " dy: " + String(dy));
 
   // 原地踢球模式
   if( kick_mode == "kick_place_mode"){
-    aim_ball(dx,dy);
+    cam_aim_ball(dy); // 相机瞄准
-    place_kick_ball(dy);
+    joyx_aim_ball(dx); // 控制左右 使小车正面是否朝向物体
-  }
-  
-  // 运动踢球模式
-  if(kick_mode == "kick_run_mode"){
-    
-    aim_ball(dx,dy);
-    
-    if(kick_over_count){ // 踢好球之后阶段，后退
-      dy_to_joyy = -50;
-    }else{
-      chase_and_kick_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){ 
-
-      dy_to_joyy = 9999;
-      kick_ball_core(); // 踢球
-      kick_over_count = 1; // 计数用于后退
-    
-    }else{ // 通过pid 向前运动
-
-      ob_y = ob_ball_dy - dy; 
-      dy_to_joyy = pid_trace_joyy(ob_y);
-
-      ready_kick_ball(); // 准备踢球
-
-    }
-}
-
-// 原地踢球模式的踢球
-void place_kick_ball(int dy){
-
     if(current_s2 < place_ball_s2 && dy > place_kick_ball_dy && dy < 120){ // 球到车身下方了，打球
       kick_ball_core();
     }else { // 追踪到目标
@@ -1656,6 +1637,87 @@ void place_kick_ball(int dy){
     }
   }
   
+  // 运动踢球模式
+  if(kick_mode == "kick_run_mode"){
+    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{
+      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 align_yaw_zero(){
+
+  // 获取当前偏航角 & 计算与基准角的偏差
+  // 小车正对人，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));
+
+  if(abs(yaw_error) < yaw_align_threshold){
+      Serial.println("aligned");
+      // 偏航角已对齐：更新状态为“已对齐”
+      yaw_to_joyx = 9999; // 重置偏航对齐控制值
+      align_flag = true; // 已经对齐偏航角了
+      pid_yaw_align.error_prev = 0.0f; // 重置偏航对齐控制器状态
+
+  } 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 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");
+  }
+}
+
 // 踢足球核心函数，控制前档舵机角度为0，使足球进入射门区域
 void kick_ball_core(){
     setFrontierServoAngles(0); // 踢球
@@ -1663,37 +1725,17 @@ void kick_ball_core(){
 }
 // 准备踢球，打开挡板，收到后方
 void ready_kick_ball(){
-    setFrontierServoAngles(100); // 打开挡板，收到后方
+  if( abs(current_s1 - ready_kick_angel) > 5){
-    leg_height_base = 30.0f; // 下降
+    setFrontierServoAngles(ready_kick_angel); // 打开挡板，收到后方
+    leg_height_base = ready_leg_height_base; // 下降
+  }
 }
 
-
+// 相机瞄准
-void aim_ob ( int dx, int dy, String ob){
+void cam_aim_ball (int dy){
-  //----- 控制左右 使小车正面是否朝向物体--------
+  if(dy == 9999){
-  // 物体在左方 dx趋近-150
+    return;
-  // 物体在右方 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;
-    
-  }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 joyx_aim_ball (int dx){
+  if(dx == 9999){
+    return;
   }
-void aim_ball ( int dx, int dy){
+  //----- 控制左右 使小车正面是否朝向物体--------
-    aim_ob ( dx, dy, "ball");
+  // 物体在左方 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;
   }
-void aim_face ( int dx, int dy){
-    aim_ob ( dx, dy, "face");
 }
 
 // 控制摄像头舵机
@@ -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状态
+  }
 }