自瞄选板修改

CMakeLists.txt

@@ -4,6 +4,7 @@ set(CMAKE_SYSTEM_VERSION 1)
 cmake_minimum_required(VERSION 3.25)
 
 # specify cross-compilers and tools
+set(CMAKE_C_COMPILER arm-none-eabi-gcc)
 set(CMAKE_CXX_COMPILER arm-none-eabi-g++)
 set(CMAKE_ASM_COMPILER  arm-none-eabi-gcc)
 set(CMAKE_AR arm-none-eabi-ar)
@@ -51,7 +52,7 @@ endif ()
 include_directories(Inc Drivers/STM32F4xx_HAL_Driver/Inc Drivers/STM32F4xx_HAL_Driver/Inc/Legacy Middlewares/Third_Party/FreeRTOS/Source/include Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F Middlewares/ST/STM32_USB_Device_Library/Core/Inc Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc Drivers/CMSIS/Device/ST/STM32F4xx/Include Drivers/CMSIS/Include Middlewares/ST/ARM/DSP/Inc
         bsp bsp/adc bsp/can bsp/dwt bsp/flash bsp/gpio bsp/iic bsp/log bsp/pwm bsp/spi bsp/usart bsp/usb
         modules modules/alarm modules/algorithm modules/BMI088 modules/can_comm modules/daemon modules/encoder modules/imu modules/ist8310
-        modules/led modules/master_machine modules/message_center modules/motor modules/oled modules/referee modules/remote modules/standard_cmd
+        modules/led modules/master_machine modules/message_center modules/motor modules/oled modules/power_meter modules/referee modules/remote modules/standard_cmd
         modules/super_cap modules/TFminiPlus modules/unicomm modules/vofa modules/auto_aim
         modules/motor/DJImotor modules/motor/DMmotor modules/motor/HTmotor modules/motor/LKmotor modules/motor/servo_motor modules/motor/step_motor
         application application/chassis application/cmd application/gimbal application/shoot
@@ -73,8 +74,7 @@ add_link_options(-Wl,-gc-sections,--print-memory-usage,-Map=${PROJECT_BINARY_DIR
 add_link_options(-mcpu=cortex-m4 -mthumb -mthumb-interwork)
 add_link_options(-T ${LINKER_SCRIPT})
 
-add_executable(${PROJECT_NAME}.elf ${SOURCES} ${LINKER_SCRIPT}
+add_executable(${PROJECT_NAME}.elf ${SOURCES} ${LINKER_SCRIPT})
-        modules/power_meter/power_meter.c)
 
 target_link_libraries(${PROJECT_NAME}.elf ${CMAKE_SOURCE_DIR}/Middlewares/ST/ARM/DSP/Lib/libarm_cortexM4lf_math.a)
 

application/cmd/robot_cmd.c

@@ -30,6 +30,7 @@ static Publisher_t *chassis_cmd_pub;   // 底盘控制消息发布者
 static Subscriber_t *chassis_feed_sub; // 底盘反馈信息订阅者
 #endif                                 // ONE_BOARD
 static float temp_yaw; //for test
+static float temp_yaw_err; //for test
 static int temp_index; //for test
 
 static Chassis_Ctrl_Cmd_s chassis_cmd_send;      // 发送给底盘应用的信息,包括控制信息和UI绘制相关
@@ -165,7 +166,7 @@ static void pitch_limit()
 }
 static void auto_aim_mode()
 {
-    trajectory_cal.v0 = 14.5f; //弹速30
+    trajectory_cal.v0 = 13.5f; //弹速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;
@@ -199,20 +200,30 @@ static void auto_aim_mode()
 
         gimbal_cmd_send.pitch = -trajectory_cal.cmd_pitch * 180 / PI + 0.2 * trajectory_cal.dis;
 
-        //float target_yaw = -atan2f(aim_select.armor_pose[target_index].x,
+        float target_yaw = atan2f(aim_select.armor_pose[target_index].y, aim_select.armor_pose[target_index].x) * 180 / PI;
-//                                  aim_select.armor_pose[target_index].y);
 
         //float target_yaw = aim_select.armor_pose[target_index].yaw * 180 / PI;
-        float target_yaw = trajectory_cal.cmd_yaw * 180 / PI;
+        //float target_yaw = trajectory_cal.cmd_yaw * 180 / PI;
         temp_yaw = target_yaw; //for test
 
         float yaw_err = fabsf(target_yaw - gimbal_fetch_data.gimbal_imu_data.Yaw);
+        temp_yaw_err = yaw_err;
+        if(vision_recv_data->armors_num == 3){
             if (yaw_err <= 2) //3度
             {
                 aim_select.suggest_fire = 1;
             }
             else
                 aim_select.suggest_fire = 0;
+        }else{
+            if (yaw_err <= 4) //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度

application/robot_def.h

@@ -26,7 +26,7 @@
 
 /* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */
 // 云台参数
-#define YAW_CHASSIS_ALIGN_ECD 4.386964  // 云台和底盘对齐指向相同方向时的电机position值,需要加上PI，因为单圈角度计算里增加了PI若对云台有机械改动需要修改
+#define YAW_CHASSIS_ALIGN_ECD 3.641593  // 云台和底盘对齐指向相同方向时的电机position值,需要加上PI，因为单圈角度计算里增加了PI若对云台有机械改动需要修改
 #define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度
 #define PITCH_HORIZON_ECD 3412      // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
 #define PITCH_MAX_ANGLE 13           // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)

modules/auto_aim/auto_aim.c

@@ -58,43 +58,15 @@ int aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_c
             aim_sel->armor_pose[i].yaw = aim_sel->target_state.yaw + i * (2.0f * PI / 3.0f);
             use_1 = !use_1;
         }
-
+        // 选择较近的装甲板
-        // 选择两块较近的装甲板
         float distance[3];
+        int label = 0;
         for (i = 0; i < 3; i++) {
             distance[i] = powf(aim_sel->armor_pose[i].x, 2) + powf(aim_sel->armor_pose[i].y, 2);
+            if(distance[i]<distance[label])
+                label = i;
         }
-
+        idx = label;
-        int label_first = 0;
-        int label_second = 1;
-
-        if(distance[label_first] > distance[label_second])
-        {
-            label_first = 1;
-            label_second = 0;
-        }
-
-        if(distance[2]<distance[label_second])
-            label_second = 2;
-
-
-        //再选择两块装甲板与自身位置连线，同旋转中心自身位置连线，夹角较小的为目标装甲板
-        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.0f;
@@ -253,41 +225,16 @@ int aim_center_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_
 //            } else label_second = 1;
 //        }
 
-        // 选择两块较近的装甲板
+        // 选择较近的装甲板
         float distance[3];
+        int label = 0;
         for (i = 0; i < 3; i++) {
             distance[i] = powf(aim_sel->armor_pose[i].x, 2) + powf(aim_sel->armor_pose[i].y, 2);
+            if(distance[i]<distance[label])
+                label = i;
         }
 
-        int label_first = 0;
+        idx = label;
-        int label_second = 1;
-
-        if(distance[label_first] > distance[label_second])
-        {
-            label_first = 1;
-            label_second = 0;
-        }
-
-        if(distance[2]<distance[label_second])
-            label_second = 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.0f;