自瞄上操作端并优化，pitch修正失败，ui刷新完成

application/chassis/chassis.c

@@ -160,10 +160,10 @@ void ChassisInit()
  */
 static void MecanumCalculate()
 {
-    vt_lf = chassis_vx + chassis_vy - chassis_cmd_recv.wz * (LF_CENTER-1);
+    vt_lf = chassis_vx + chassis_vy - chassis_cmd_recv.wz * (LF_CENTER);
-    vt_rf = -chassis_vx + chassis_vy + chassis_cmd_recv.wz * (RF_CENTER-1);
+    vt_rf = -chassis_vx + chassis_vy + chassis_cmd_recv.wz * (RF_CENTER);
-    vt_lb = chassis_vx - chassis_vy - chassis_cmd_recv.wz * (-LB_CENTER-1);
+    vt_lb = chassis_vx - chassis_vy - chassis_cmd_recv.wz * (-LB_CENTER);
-    vt_rb = -chassis_vx - chassis_vy + chassis_cmd_recv.wz * (-RB_CENTER-1);
+    vt_rb = -chassis_vx - chassis_vy + chassis_cmd_recv.wz * (-RB_CENTER);
 }
 
 
@@ -255,13 +255,13 @@ void ChassisTask()
         break;
     case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出
         chassis_cmd_recv.wz = 2.0f * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle);
-        if(chassis_cmd_recv.wz > 2000)
+        if(chassis_cmd_recv.wz > 3000)
-            chassis_cmd_recv.wz = 2000;
+            chassis_cmd_recv.wz = 3000;
-        if(chassis_cmd_recv.wz < -2000)
+        if(chassis_cmd_recv.wz < -3000)
-            chassis_cmd_recv.wz = -2000;
+            chassis_cmd_recv.wz = -3000;
         break;
     case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
-        chassis_cmd_recv.wz = 2300;
+        chassis_cmd_recv.wz = 3000;
         break;
     default:
         break;

application/cmd/robot_cmd.c

@@ -136,6 +136,58 @@ static void pitch_limit()
     if(gimbal_cmd_send.pitch>18) gimbal_cmd_send.pitch = 18;
     if(gimbal_cmd_send.pitch<-38) gimbal_cmd_send.pitch = -38;
 }
+static void hand_aim_mode()
+{
+    gimbal_cmd_send.yaw += (float)rc_data[TEMP].mouse.x / 660 * 9; // 系数待测
+    gimbal_cmd_send.pitch += (float)rc_data[TEMP].mouse.y / 660 * 9;
+    pitch_limit();
+}
+static void auto_aim_mode()
+{
+    trajectory_cal.v0 = 30; //弹速30
+    if (vision_recv_data->x == 0 && vision_recv_data->y == 0 && vision_recv_data->z == 0
+        && vision_recv_data->vx == 0 && vision_recv_data->vy == 0 && vision_recv_data->vz == 0) {
+        aim_select.suggest_fire = 0;
+        //未发现目标
+        no_find_cnt++;
+
+        if (no_find_cnt >= 2000) {
+            //gimbal_scan_flag = 1;
+            //auto_aim_flag = 0;
+        }
+        //else
+        //auto_aim_flag = 1;
+    } else {
+        //弹道解算
+        no_find_cnt = 0;
+        auto_aim_flag = 1;
+
+        target_index = auto_aim(&aim_select, &trajectory_cal, vision_recv_data);
+        VisionSetAim(aim_select.aim_point[0],aim_select.aim_point[1],aim_select.aim_point[2]);
+
+        gimbal_cmd_send.yaw = -trajectory_cal.cmd_yaw * 180 / PI;
+
+        gimbal_cmd_send.pitch = -trajectory_cal.cmd_pitch * 180 / PI;
+
+        float target_yaw = atan2f(aim_select.armor_pose[target_index].x,
+                                  aim_select.armor_pose[target_index].y);
+        float yaw_err = fabsf(target_yaw - gimbal_fetch_data.gimbal_imu_data.Yaw);
+        if (yaw_err <= 3) //3度
+        {
+            aim_select.suggest_fire = 1;
+        }
+        else
+            aim_select.suggest_fire = 0;
+
+//            float yaw_err = fabsf(trajectory_cal.cmd_yaw - gimbal_cmd_send.yaw);
+//            if (yaw_err <= 3) //3度
+//                aim_select.suggest_fire = 1;
+//            else
+//                aim_select.suggest_fire = 0;
+    }
+}
+
+
 /**
  * @brief 控制输入为遥控器(调试时)的模式和控制量设置
  *
@@ -169,45 +221,7 @@ static void RemoteControlSet()
     }
     // 左侧开关状态为[下],视觉模式
     if (switch_is_down(rc_data[TEMP].rc.switch_left)) {
-        trajectory_cal.v0 = 12.5; //弹速30
+        auto_aim_mode();
-        if (vision_recv_data->x == 0 && vision_recv_data->y == 0 && vision_recv_data->z == 0
-            && vision_recv_data->vx == 0 && vision_recv_data->vy == 0 && vision_recv_data->vz == 0) {
-            aim_select.suggest_fire = 0;
-            //未发现目标
-            no_find_cnt++;
-
-            if (no_find_cnt >= 2000) {
-                //gimbal_scan_flag = 1;
-                //auto_aim_flag = 0;
-            }
-            //else
-            //auto_aim_flag = 1;
-        } else {
-            //弹道解算
-            no_find_cnt = 0;
-            auto_aim_flag = 1;
-
-            target_index = auto_aim(&aim_select, &trajectory_cal, vision_recv_data);
-            VisionSetAim(aim_select.aim_point[0],aim_select.aim_point[1],aim_select.aim_point[2]);
-
-            gimbal_cmd_send.yaw = -trajectory_cal.cmd_yaw * 180 / PI;
-
-            gimbal_cmd_send.pitch = -trajectory_cal.cmd_pitch * 180 / PI;
-
-            float target_yaw = atan2f(aim_select.armor_pose[target_index].x,
-                                      aim_select.armor_pose[target_index].y);
-            float yaw_err = fabsf(target_yaw - gimbal_fetch_data.gimbal_imu_data.Yaw);
-            if (yaw_err <= 3) //3度
-                aim_select.suggest_fire = 1;
-            else
-                aim_select.suggest_fire = 0;
-
-//            float yaw_err = fabsf(trajectory_cal.cmd_yaw - gimbal_cmd_send.yaw);
-//            if (yaw_err <= 3) //3度
-//                aim_select.suggest_fire = 1;
-//            else
-//                aim_select.suggest_fire = 0;
-        }
     }
     // 云台软件限位
 
@@ -242,12 +256,10 @@ static void RemoteControlSet()
  */
 static void MouseKeySet()
 {
-    chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].a * 6000 - rc_data[TEMP].key[KEY_PRESS].d * 6000; // 系数待测
+    chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].a * 20000 - rc_data[TEMP].key[KEY_PRESS].d * 20000; // 系数待测
-    chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].w * 6000 - rc_data[TEMP].key[KEY_PRESS].s * 6000;
+    chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].w * 20000 - rc_data[TEMP].key[KEY_PRESS].s * 20000;
+
 
-    gimbal_cmd_send.yaw += (float)rc_data[TEMP].mouse.x / 660 * 9; // 系数待测
-    gimbal_cmd_send.pitch += (float)rc_data[TEMP].mouse.y / 660 * 9;
-    pitch_limit();
 //    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Z] % 3) // Z键设置弹速//
 //    {
 //    case 0:
@@ -260,25 +272,52 @@ static void MouseKeySet()
 //        shoot_cmd_send.bullet_speed = 30;
 //        break;
 //    }
+
+    switch (rc_data[TEMP].mouse.press_r) // 右键自瞄模式
+    {
+        case 1:
+            if((vision_recv_data->x == 0 && vision_recv_data->y == 0 && vision_recv_data->z == 0
+                && vision_recv_data->vx == 0 && vision_recv_data->vy == 0 &&
+                vision_recv_data->vz == 0))
+                hand_aim_mode();
+            else
+                auto_aim_mode();
+            break;
+        default:
+            hand_aim_mode();
+            break;
+    }
+
     switch (rc_data[TEMP].mouse.press_l) // 左键发射模式
     {
     case 1:
         if(shoot_cmd_send.friction_mode == FRICTION_ON)
+        {
+            if(rc_data[TEMP].mouse.press_r && rc_data[TEMP].mouse.press_l)
+            {
+                if(aim_select.suggest_fire == 1)
                     shoot_cmd_send.load_mode = LOAD_1_BULLET;
                 else
                     shoot_cmd_send.load_mode = LOAD_STOP;
                 break;
+            }
+            shoot_cmd_send.load_mode = LOAD_1_BULLET;
+        }
+        else
+            shoot_cmd_send.load_mode = LOAD_STOP;
+        break;
     default:
         shoot_cmd_send.load_mode = LOAD_STOP;
         break;
     }
-    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_R] % 2) // R键开关弹舱
+
+    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_R] % 2) // R键手动刷新UI
     {
-    case 0:
+    case 1:
-        shoot_cmd_send.lid_mode = LID_OPEN;
+        MyUIInit();
+        rc_data[TEMP].key_count[KEY_PRESS][Key_R]++;
         break;
     default:
-        shoot_cmd_send.lid_mode = LID_CLOSE;
         break;
     }
     switch (rc_data[TEMP].key_count[KEY_PRESS][Key_F] % 2) // F键开关摩擦轮
@@ -290,7 +329,7 @@ static void MouseKeySet()
         shoot_cmd_send.friction_mode = FRICTION_ON;
         break;
     }
-    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Q] % 2) // Q键开关小陀螺
+    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_E] % 2) // E键开关小陀螺
     {
         case 0:
             chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
@@ -301,21 +340,21 @@ static void MouseKeySet()
             gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
             break;
     }
-    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_C] % 4) // C键设置底盘速度
+//    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_C] % 4) // C键设置底盘速度
-    {
+//    {
-    case 0:
+//    case 0:
-        chassis_cmd_send.chassis_speed_buff = 40;
+//        chassis_cmd_send.chassis_speed_buff = 40;
-        break;
+//        break;
-    case 1:
+//    case 1:
-        chassis_cmd_send.chassis_speed_buff = 60;
+//        chassis_cmd_send.chassis_speed_buff = 60;
-        break;
+//        break;
-    case 2:
+//    case 2:
-        chassis_cmd_send.chassis_speed_buff = 80;
+//        chassis_cmd_send.chassis_speed_buff = 80;
-        break;
+//        break;
-    default:
+//    default:
-        chassis_cmd_send.chassis_speed_buff = 100;
+//        chassis_cmd_send.chassis_speed_buff = 100;
-        break;
+//        break;
-    }
+//    }
     switch (rc_data[TEMP].key[KEY_PRESS].shift) // 待添加 按shift允许超功率 消耗缓冲能量
     {
     case 1:

application/gimbal/gimbal.c

@@ -27,10 +27,9 @@ void GimbalInit()
         },
         .controller_param_init_config = {
             .angle_PID = {
-                .Kp = 40, // 8
+                .Kp = 200, // 8
                 .Ki = 0,
-                .Kd = 5,
+                .Kd = 20,
-                .DeadBand = 0.1,
                 .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                 .IntegralLimit = 100,
 
@@ -38,8 +37,8 @@ void GimbalInit()
             },
             .speed_PID = {
                 .Kp = 70,  // 70
-                .Ki = 200, // 200
+                .Ki = 0, // 200
-                .Kd = 10,//10
+                .Kd = 0,//10
                 .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                 .IntegralLimit = 3000,
                 .MaxOut = 20000,
@@ -65,8 +64,8 @@ void GimbalInit()
         },
         .controller_param_init_config = {
             .angle_PID = {
-                .Kp = 15, // 10
+                .Kp = 20, // 65
-                .Ki = 0.1f,
+                .Ki = 0,
                 .Kd = 1,
                 .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                 .IntegralLimit = 100,//100
@@ -75,7 +74,7 @@ void GimbalInit()
             .speed_PID = {
                 .Kp = 30,  // 50
                 .Ki = 0, // 350
-                .Kd = 1,   // 0
+                .Kd = 2,   // 0
                 .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                 .IntegralLimit = 150,//2500
                 .MaxOut = 2000,//20000

application/shoot/shoot.c

@@ -55,15 +55,15 @@ void ShootInit()
 
             .outer_loop_type = SPEED_LOOP,
             .close_loop_type = SPEED_LOOP ,
-            .motor_reverse_flag = MOTOR_DIRECTION_REVERSE,
+            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
         },
         .motor_type = M3508};
     friction_config.can_init_config.tx_id = 3,
     friction_l = DJIMotorInit(&friction_config);
 
-    friction_config.can_init_config.tx_id = 4; // 右摩擦轮,改txid和方向就行
+//    friction_config.can_init_config.tx_id = 4; // 右摩擦轮,改txid和方向就行
-    friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
+//    friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
-    friction_r = DJIMotorInit(&friction_config);
+//    friction_r = DJIMotorInit(&friction_config);
 
     friction_config.can_init_config.tx_id = 6; // 上摩擦轮,改txid和方向就行
     friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
@@ -135,14 +135,14 @@ void ShootTask()
     if (shoot_cmd_recv.shoot_mode == SHOOT_OFF)
     {
         DJIMotorStop(friction_l);
-        DJIMotorStop(friction_r);
+//        DJIMotorStop(friction_r);
         DJIMotorStop(friction_z);
         DJIMotorStop(loader);
     }
     else // 恢复运行
     {
         DJIMotorEnable(friction_l);
-        DJIMotorEnable(friction_r);
+//        DJIMotorEnable(friction_r);
         DJIMotorEnable(friction_z);
         DJIMotorEnable(loader);
     }
@@ -193,14 +193,14 @@ void ShootTask()
     // 确定是否开启摩擦轮,后续可能修改为键鼠模式下始终开启摩擦轮(上场时建议一直开启)
     if (shoot_cmd_recv.friction_mode == FRICTION_ON)
     {
-            DJIMotorSetRef(friction_l, 39000);
+            DJIMotorSetRef(friction_l, 36000);
-            DJIMotorSetRef(friction_r, 39000);
+//            DJIMotorSetRef(friction_r, 39000);
-            DJIMotorSetRef(friction_z, 39000);//39000/6 = 6500
+            DJIMotorSetRef(friction_z, 36000);//39000/6 = 6500
     }
     else // 关闭摩擦轮
     {
         DJIMotorSetRef(friction_l, 0);
-        DJIMotorSetRef(friction_r, 0);
+//        DJIMotorSetRef(friction_r, 0);
         DJIMotorSetRef(friction_z, 0);
     }
 

modules/auto_aim/auto_aim.c

@@ -3,14 +3,203 @@
 //
 #include "auto_aim.h"
 #include "arm_math.h"
+uint8_t mode_flag=0;
+/**
+ * @brief          选择目标装甲板（瞄装甲板）
+ * @author         SJQ
+ * @param[in]      trajectory_cal:弹道解算结构体
+ * @retval         返回空
+ */
+int aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal) {
+    aim_sel->delay_time = trajectory_cal->fly_time + trajectory_cal->extra_delay_time;
+    aim_sel->target_state.yaw += aim_sel->target_state.v_yaw * aim_sel->delay_time;
 
+    //计算四块装甲板的位置
+    int use_1 = 1;
+    int i = 0;
+    int idx = 0; // 选择的装甲板
+    // 为平衡步兵
+    if (aim_sel->target_state.armor_num == 2) {
+        for (i = 0; i < 2; i++) {
+            float tmp_yaw = aim_sel->target_state.yaw + i * PI;
+            float r = aim_sel->target_state.r1;
+
+            aim_sel->armor_pose[i].x = aim_sel->target_state.x - r * arm_cos_f32(tmp_yaw);
+            aim_sel->armor_pose[i].y = aim_sel->target_state.y - r * arm_sin_f32(tmp_yaw);
+            aim_sel->armor_pose[i].z = aim_sel->target_state.z;
+            aim_sel->armor_pose[i].yaw = aim_sel->target_state.yaw + i * PI;
+        }
+
+//        float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw);
+//
+//        //因为是平衡步兵 只需判断两块装甲板即可
+//        float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[1].yaw);
+//        if (temp_yaw_diff < yaw_diff_min) {
+//            yaw_diff_min = temp_yaw_diff;
+//            idx = 1;
+//        }
+        // 平衡步兵选择两块装甲板中较近的装甲板
+        float distance_min = powf(aim_sel->armor_pose[0].x, 2) + powf(aim_sel->armor_pose[0].y, 2);
+        float distance_temp = powf(aim_sel->armor_pose[1].x, 2) + powf(aim_sel->armor_pose[1].y, 2);
+        if (distance_temp < distance_min) {
+            distance_min = distance_temp;
+            idx = 1;
+        }
+
+    } else if (aim_sel->target_state.armor_num == 3)//前哨站
+    {
+        for (i = 0; i < 3; i++) {
+            float tmp_yaw;
+            if (aim_sel->target_state.v_yaw <= 0)
+                tmp_yaw = aim_sel->target_state.yaw + i * (2.0 * PI / 3.0);
+            else
+                tmp_yaw = aim_sel->target_state.yaw - i * (2.0 * PI / 3.0);
+            float r = use_1 ? aim_sel->target_state.r1 : aim_sel->target_state.r2;
+            aim_sel->armor_pose[i].x = aim_sel->target_state.x - r * cos(tmp_yaw);
+            aim_sel->armor_pose[i].y = aim_sel->target_state.y - r * sin(tmp_yaw);
+            aim_sel->armor_pose[i].z = use_1 ? aim_sel->target_state.z : aim_sel->target_state.z +
+                                                                         aim_sel->target_state.dz;
+            aim_sel->armor_pose[i].yaw = aim_sel->target_state.yaw + i * (2.0 * PI / 3.0);
+            use_1 = !use_1;
+        }
+
+//        //计算枪管到目标装甲板yaw最小的那个装甲板
+//        float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw);
+//        for (i = 1; i < 3; i++) {
+//            float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[i].yaw);
+//            if (temp_yaw_diff < yaw_diff_min) {
+//                yaw_diff_min = temp_yaw_diff;
+//                idx = i;
+//            }
+//        }
+        // 选择两块较近的装甲板，再选择两块装甲板与自身位置连线，同旋转中心自身位置连线，夹角较小的为目标装甲板
+//         float distance_temp0 = powf(aim_sel->armor_pose[0].x, 2) + powf(aim_sel->armor_pose[0].y, 2);
+//         float distance_temp1 = powf(aim_sel->armor_pose[1].x, 2) + powf(aim_sel->armor_pose[1].y, 2);
+//         float distance_temp2 = powf(aim_sel->armor_pose[2].x, 2) + powf(aim_sel->armor_pose[2].y, 2);
+
+//        int label_first = 0;
+//        int label_second = 1;
+//        if (distance_temp0 > distance_temp1) {
+//            label_first = 1;
+//            if (distance_temp0 > distance_temp2) {
+//                label_second = 2;
+//            } else label_second = 0;
+//        } else {
+//            label_first = 0;
+//            if (distance_temp1 > distance_temp2) {
+//                label_second = 2;
+//            } else label_second = 1;
+//        }
+
+        // 选择两块较近的装甲板
+        float distance[3];
+        for (i = 0; i < 3; i++) {
+            distance[i] = powf(aim_sel->armor_pose[i].x, 2) + powf(aim_sel->armor_pose[i].y, 2);
+        }
+
+        int label_first = 0;
+        int label_second = 0;
+
+        for (i = 1; i < 3; i++) {
+            if (distance[i] < distance[label_first]) {
+                label_second = label_first;
+                label_first = i;
+            } else if (distance[i] < distance[label_second]) {
+                label_second = i;
+            } else if (distance[i] < distance[label_second] && distance[i] < distance[label_first]) {
+                label_second = i;
+            }
+        }
+
+        //再选择两块装甲板与自身位置连线，同旋转中心自身位置连线，夹角较小的为目标装甲板
+        float center_length = sqrtf(powf(aim_sel->target_state.x, 2) + powf(aim_sel->target_state.y, 2));
+
+        float cos_theta_first = (aim_sel->armor_pose[label_first].x * aim_sel->target_state.x +
+                                 aim_sel->armor_pose[label_first].y * aim_sel->target_state.y) /
+                                (sqrtf(powf(aim_sel->armor_pose[label_first].x, 2) +
+                                       powf(aim_sel->armor_pose[label_first].y, 2)) * center_length);
+        float cos_theta_second = (aim_sel->armor_pose[label_second].x * aim_sel->target_state.x +
+                                  aim_sel->armor_pose[label_second].y * aim_sel->target_state.y) /
+                                 (sqrtf(powf(aim_sel->armor_pose[label_second].x, 2) +
+                                        powf(aim_sel->armor_pose[label_second].y, 2)) * center_length);
+
+        if (cos_theta_first > cos_theta_second)
+            idx = label_first;
+        else
+            idx = label_second;
+
+    } else {
+        for (i = 0; i < 4; i++) {
+            float tmp_yaw = aim_sel->target_state.yaw + i * PI / 2.0;
+            float r = use_1 ? aim_sel->target_state.r1 : aim_sel->target_state.r2;
+
+            aim_sel->armor_pose[i].x = aim_sel->target_state.x - r * arm_cos_f32(tmp_yaw);
+            aim_sel->armor_pose[i].y = aim_sel->target_state.y - r * arm_sin_f32(tmp_yaw);
+            aim_sel->armor_pose[i].z = use_1 ? aim_sel->target_state.z : aim_sel->target_state.z +
+                                                                         aim_sel->target_state.dz;
+            aim_sel->armor_pose[i].yaw = tmp_yaw;
+            use_1 = !use_1;
+        }
+
+//        //计算枪管到目标装甲板yaw最小的那个装甲板
+//        float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw);
+//        for (i = 1; i < 4; i++) {
+//            float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[i].yaw);
+//            if (temp_yaw_diff < yaw_diff_min) {
+//                yaw_diff_min = temp_yaw_diff;
+//                idx = i;
+//            }
+//        }
+
+        // 选择两块较近的装甲板
+        float distance[4];
+        for (i = 0; i < 4; i++) {
+            distance[i] = powf(aim_sel->armor_pose[i].x, 2) + powf(aim_sel->armor_pose[i].y, 2);
+        }
+
+        int label_first = 0;
+        int label_second = 0;
+
+        for (i = 1; i < 4; i++) {
+            if (distance[i] < distance[label_first]) {
+                label_second = label_first;
+                label_first = i;
+            } else if (distance[i] < distance[label_second]) {
+                label_second = i;
+            } else if (distance[i] < distance[label_second] && distance[i] < distance[label_first]) {
+                label_second = i;
+            }
+        }
+
+        //再选择两块装甲板与自身位置连线，同旋转中心自身位置连线，夹角较小的为目标装甲板
+        float center_length = sqrtf(powf(aim_sel->target_state.x, 2) + powf(aim_sel->target_state.y, 2));
+
+        float cos_theta_first = (aim_sel->armor_pose[label_first].x * aim_sel->target_state.x +
+                                 aim_sel->armor_pose[label_first].y * aim_sel->target_state.y) /
+                                (sqrtf(powf(aim_sel->armor_pose[label_first].x, 2) +
+                                       powf(aim_sel->armor_pose[label_first].y, 2)) * center_length);
+        float cos_theta_second = (aim_sel->armor_pose[label_second].x * aim_sel->target_state.x +
+                                  aim_sel->armor_pose[label_second].y * aim_sel->target_state.y) /
+                                 (sqrtf(powf(aim_sel->armor_pose[label_second].x, 2) +
+                                        powf(aim_sel->armor_pose[label_second].y, 2)) * center_length);
+
+        if (cos_theta_first > cos_theta_second)
+            idx = label_first;
+        else
+            idx = label_second;
+    }
+    aim_sel->aim_point[0] = aim_sel->armor_pose[idx].x + aim_sel->target_state.vx * aim_sel->delay_time;
+    aim_sel->aim_point[1] = aim_sel->armor_pose[idx].y + aim_sel->target_state.vy * aim_sel->delay_time;
+    aim_sel->aim_point[2] = aim_sel->armor_pose[idx].z + aim_sel->target_state.vz * aim_sel->delay_time;
+    return idx;
+}
 /**
  * @brief          选择目标装甲板
  * @author         SJQ
  * @param[in]      trajectory_cal:弹道解算结构体
  * @retval         返回空
  */
-int aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal) {
+int aim_center_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal) {
     aim_sel->delay_time = trajectory_cal->fly_time + trajectory_cal->extra_delay_time;
     aim_sel->target_state.yaw += aim_sel->target_state.v_yaw * aim_sel->delay_time;
 
@@ -229,7 +418,7 @@ void get_cmd_angle(Trajectory_Type_t *trajectory_cal) {
     arm_power_f32(trajectory_cal->position_xy, 2, &trajectory_cal->dis2);
     arm_sqrt_f32(trajectory_cal->dis2, &trajectory_cal->dis);
 
-    trajectory_cal->dis = trajectory_cal->dis - 0.20;
+//    trajectory_cal->dis = trajectory_cal->dis - 0.20;
 
     trajectory_cal->theta_0 = atan2f(trajectory_cal->z, trajectory_cal->dis);
     trajectory_cal->alpha = atan2f(trajectory_cal->position_xy[1], trajectory_cal->position_xy[0]);
@@ -264,7 +453,7 @@ void get_cmd_angle(Trajectory_Type_t *trajectory_cal) {
 }
 
 int auto_aim(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal, RecievePacket_t *receive_packet) {
-    trajectory_cal->extra_delay_time = 0.025f;//0.025
+    trajectory_cal->extra_delay_time = 0.035f;//0.025
 
     aim_sel->target_state.armor_type = receive_packet->id;
     aim_sel->target_state.armor_num = receive_packet->armors_num;
@@ -280,7 +469,17 @@ int auto_aim(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal, Reci
     aim_sel->target_state.r2 = receive_packet->r2;
     aim_sel->target_state.dz = receive_packet->dz;
 
-    int idx = aim_armor_select(aim_sel, trajectory_cal);
+    int idx = 0;
+    if(aim_sel->target_state.v_yaw >= 0.7* PI)
+    {
+        mode_flag=0;
+         idx = aim_center_select(aim_sel, trajectory_cal);
+    }
+    else
+    {
+        mode_flag=1;
+         idx = aim_armor_select(aim_sel, trajectory_cal);
+    }
 
     trajectory_cal->position_xy[0] = aim_sel->aim_point[0];
     trajectory_cal->position_xy[1] = aim_sel->aim_point[1];

modules/referee/referee_task.c

@@ -62,8 +62,8 @@ static uint32_t shoot_line_location[10] = {540, 960, 490, 515, 565};
 
 void MyUIInit()
 {
-//    if (!referee_recv_info->init_flag)
+    if (!referee_recv_info->init_flag)
-//        vTaskDelete(NULL); // 如果没有初始化裁判系统则直接删除ui任务
+        vTaskDelete(NULL); // 如果没有初始化裁判系统则直接删除ui任务
     while (referee_recv_info->GameRobotState.robot_id == 0)
         osDelay(100); // 若还未收到裁判系统数据,等待一段时间后再检查
 
@@ -94,9 +94,9 @@ void MyUIInit()
 
     // 绘制车辆状态标志，动态
     // 由于初始化时xxx_last_mode默认为0，所以此处对应UI也应该设为0时对应的UI，防止模式不变的情况下无法置位flag，导致UI无法刷新
-    UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_ADD, 8, UI_Color_Main, 15, 2, 270, 750, "zeroforce");
+    UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_ADD, 8, UI_Color_Main, 15, 2, 270, 750, "follow");
     UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[0]);
-    UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_ADD, 8, UI_Color_Yellow, 15, 2, 270, 700, "zeroforce");
+    UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_ADD, 8, UI_Color_Yellow, 15, 2, 270, 700, "free");
     UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[1]);
     UICharDraw(&UI_State_dyn[2], "sd2", UI_Graph_ADD, 8, UI_Color_Orange, 15, 2, 270, 650, "off");
     UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[2]);

modules/referee/rm_referee.h

@@ -68,6 +68,7 @@ typedef struct
 	lid_mode_e lid_mode;					 // 弹舱盖打开
 	Chassis_Power_Data_s Chassis_Power_Data; // 功率控制
 
+
 	// 上一次的模式，用于flag判断
 	chassis_mode_e chassis_last_mode;
 	gimbal_mode_e gimbal_last_mode;