5.14提交

application/cmd/robot_cmd.c

@@ -17,7 +17,7 @@
 
 // 私有宏,自动将编码器转换成角度值
 #define YAW_ALIGN_ANGLE (YAW_CHASSIS_ALIGN_ECD * ECD_ANGLE_COEF_DJI) // 对齐时的角度,0-360
-#define PTICH_HORIZON_ANGLE (PITCH_HORIZON_ECD * ECD_ANGLE_COEF_DJI) // pitch水平时电机的角度,0-360
+#define PITCH_HORIZON_ANGLE (PITCH_HORIZON_ECD * ECD_ANGLE_COEF_DJI) // pitch水平时电机的角度,0-360
 
 /* cmd应用包含的模块实例指针和交互信息存储*/
 #ifdef GIMBAL_BOARD // 对双板的兼容,条件编译
@@ -122,6 +122,60 @@ static void CalcOffsetAngle()
 #endif
 }
 
+#define PITCH_ZERO_ANGLE 195.0f //电机旋转导致的云台相对底盘角度
+static void pitch_limit()
+{
+    static float PitchMotorAngle,DeltaPitchAngle;
+    PitchMotorAngle = gimbal_fetch_data.pitch_motor_ecd - PITCH_ZERO_ANGLE;//电机旋转导致的云台相对底盘角度
+    DeltaPitchAngle = gimbal_fetch_data.gimbal_imu_data.Pitch - PitchMotorAngle;
+    gimbal_cmd_send.pitch = LIMIT_MIN_MAX(gimbal_cmd_send.pitch, PITCH_MIN_ANGLE-DeltaPitchAngle, PITCH_MAX_ANGLE-DeltaPitchAngle);
+}
+
+static void auto_aim_mode() {
+    trajectory_cal.v0 = 25; //弹速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;
+
+        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]);
+
+        float single_angle_yaw_now = gimbal_fetch_data.gimbal_imu_data.Yaw;
+        float diff_yaw = trajectory_cal.cmd_yaw * 180 / PI - single_angle_yaw_now;
+        float yaw_err = diff_yaw;
+
+        if(diff_yaw>180)
+            diff_yaw -= 360;
+        else if(diff_yaw<-180)
+            diff_yaw += 360;
+
+        gimbal_cmd_send.yaw = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle + diff_yaw;
+
+        gimbal_cmd_send.pitch = -trajectory_cal.cmd_pitch * 180 / PI;
+
+        if (yaw_err <= 3) //3度
+        {
+            aim_select.suggest_fire = 1;
+        }
+        else
+            aim_select.suggest_fire = 0;
+    }
+}
+
 /**
  * @brief 控制输入为遥控器(调试时)的模式和控制量设置
  *
@@ -137,7 +191,7 @@ static void RemoteControlSet()
     else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘跟随模式
     {
         chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
-        gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE;
+        gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
     }
 
     // 云台参数,确定云台控制数据
@@ -162,36 +216,7 @@ static void RemoteControlSet()
     }
     // 左侧开关状态为[下],视觉模式
     if (switch_is_down(rc_data[TEMP].rc.switch_left)) {
-        trajectory_cal.v0 = 30; //弹速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;
-
-            auto_aim(&aim_select, &trajectory_cal, vision_recv_data);
-
-            gimbal_cmd_send.yaw = trajectory_cal.cmd_yaw * 180 / PI;
-
-            gimbal_cmd_send.pitch = trajectory_cal.cmd_pitch * 180 / PI;
-
-            float yaw_err = fabsf(trajectory_cal.cmd_yaw - gimbal_cmd_send.yaw);
-            if (yaw_err <= 0.008) //3度
-                aim_select.suggest_fire = 1;
-            else
-                aim_select.suggest_fire = 0;
-        }
     }
     // 云台软件限位
 
@@ -235,18 +260,37 @@ static void MouseKeySet()
     gimbal_cmd_send.yaw += (float)rc_data[TEMP].mouse.x / 660 * 10; // 系数待测
     gimbal_cmd_send.pitch += (float)rc_data[TEMP].mouse.y / 660 * 10;
 
-    switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Z] % 3) // Z键设置弹速
+    aim_select.suggest_fire = 0;
+    if (rc_data[TEMP].mouse.press_l && (!rc_data[TEMP].mouse.press_r)) // 左键发射模式
     {
-    case 0:
+        if (shoot_cmd_send.friction_mode == FRICTION_ON) {
-        shoot_cmd_send.bullet_speed = 15;
+            shoot_cmd_send.shoot_mode = SHOOT_ON;
-        break;
+                shoot_cmd_send.load_mode = LOAD_BURSTFIRE;
-    case 1:
-        shoot_cmd_send.bullet_speed = 18;
-        break;
-    default:
-        shoot_cmd_send.bullet_speed = 30;
-        break;
         }
+    } else if ((!rc_data[TEMP].mouse.press_l) && (!rc_data[TEMP].mouse.press_r))
+    {
+        shoot_cmd_send.load_mode = LOAD_STOP;
+
+    }
+    if (rc_data[TEMP].mouse.press_r) // 右键自瞄模式
+    {
+        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))
+        {
+            shoot_cmd_send.load_mode = LOAD_STOP;
+        } else {
+            auto_aim_mode();
+            if (aim_select.suggest_fire == 1 && rc_data[TEMP].mouse.press_l &&
+                shoot_cmd_send.friction_mode == FRICTION_ON) {
+                shoot_cmd_send.shoot_mode = SHOOT_ON;
+                shoot_cmd_send.load_mode = LOAD_BURSTFIRE;
+            } else {
+                shoot_cmd_send.load_mode = LOAD_STOP;
+            }
+        }
+    }
+
     switch (rc_data[TEMP].key_count[KEY_PRESS][Key_E] % 4) // E键设置发射模式
     {
     case 0:
@@ -355,11 +399,11 @@ static void VTMouseKeySet()
 
     memcpy(&vt_data[LAST], &vt_data[TEMP], sizeof(VT_ctrl_t)); // 保存上一次的数据,用于按键持续按下和切换的判断
 
-    chassis_cmd_send.vx = vt_data[TEMP].key[KEY_PRESS].w * 1500 - vt_data[TEMP].key[KEY_PRESS].s * 1500; // 系数待测
+    chassis_cmd_send.vx = vt_data[TEMP].key[KEY_PRESS].w * 2000 - vt_data[TEMP].key[KEY_PRESS].s * 2000; // 系数待测
     //chassis_cmd_send.vy = vt_data[TEMP].key[KEY_PRESS].s * 800 - vt_data[TEMP].key[KEY_PRESS].d * 800;
 
-    gimbal_cmd_send.yaw -= (float)vt_data[TEMP].mouse.x / 660 * 5; // 系数待测
+    gimbal_cmd_send.yaw -= (float)vt_data[TEMP].mouse.x / 660 * 3; // 系数待测
-    gimbal_cmd_send.pitch += (float)vt_data[TEMP].mouse.y / 660 * 10;
+    gimbal_cmd_send.pitch -= (float)vt_data[TEMP].mouse.y / 660 * 3;
 
 
     switch (vt_data[TEMP].key_count[KEY_PRESS][Key_F] % 2) // F键开关摩擦轮
@@ -383,6 +427,8 @@ static void VTMouseKeySet()
             chassis_cmd_send.chassis_mode = CHASSIS_LATERAL;
             break;
     }
+    //R键手动刷新UI 发到底盘板自己处理
+    chassis_cmd_send.UI_refresh = vt_data[TEMP].key_count[KEY_PRESS][Key_R];
 }
 
 /**
@@ -395,7 +441,8 @@ static void VTMouseKeySet()
 static void EmergencyHandler()
 {
     // 拨轮的向下拨超过一半进入急停模式.注意向打时下拨轮是正
-    if (rc_data[TEMP].rc.dial > 300 || robot_state == ROBOT_STOP) // 还需添加重要应用和模块离线的判断
+    if ((rc_data[TEMP].rc.dial > 300 || chassis_fetch_data.power_management_chassis_output == 0) //死亡状态 急停
+    || robot_state == ROBOT_STOP) // 还需添加重要应用和模块离线的判断
     {
         robot_state = ROBOT_STOP;
         gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE;
@@ -440,13 +487,16 @@ void RobotCMDTask()
         MouseKeySet();
     }
 
-    else if (switch_is_mid(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],键盘控制
+    else if (switch_is_mid(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[中],图传链路
         VTMouseKeySet();
 
+
     EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况
 
-    // 设置视觉发送数据,还需增加加速度和角速度数据
+    pitch_limit();
-    // VisionSetFlag(chassis_fetch_data.enemy_color,,chassis_fetch_data.bullet_speed)
+    //设置视觉识别颜色
+    VisionSetFlag(chassis_fetch_data.enemy_color);
+    gimbal_cmd_send.chassis_rotate_wz = chassis_fetch_data.real_wz;
 
     // 推送消息,双板通信,视觉通信等
     // 其他应用所需的控制数据在remotecontrolsetmode和mousekeysetmode中完成设置
@@ -458,5 +508,4 @@ void RobotCMDTask()
 #endif // GIMBAL_BOARD
     PubPushMessage(shoot_cmd_pub, (void *)&shoot_cmd_send);
     PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send);
-    VisionSend(&vision_send_data);
 }

application/gimbal/gimbal.c

@@ -50,8 +50,8 @@ void GimbalInit()
                 .MaxOut = 500,
             },
             .speed_PID = {
-                .Kp = 1e4,  // 50
+                .Kp = 8000,//1e4,  // 50 @todo:有疯转现象  先降低试试看
-                .Ki = 5000, // 200
+                .Ki = 0,//5000, // 200
                 .Kd = 0,
                 .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                 .IntegralLimit = 10000,
@@ -201,7 +201,7 @@ void GimbalTask()
     // 设置反馈数据,主要是imu和yaw的ecd
     gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data;
     gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->measure.angle_single_round;
-    gimbal_feedback_data.pitch_motor_ecd = pitch_motor->measure.angle_single_round + pitch_motor->motor_controller.angle_PID.Err;
+    gimbal_feedback_data.pitch_motor_ecd = pitch_motor->measure.angle_single_round;
 
 
     // 推送消息

application/robot_def.h

@@ -29,8 +29,8 @@
 #define YAW_CHASSIS_ALIGN_ECD 5046  // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
 #define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度
 #define PITCH_HORIZON_ECD 3412      // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
-#define PITCH_MAX_ANGLE 0           // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
+#define PITCH_MAX_ANGLE 23.0f           // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
-#define PITCH_MIN_ANGLE 0           // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
+#define PITCH_MIN_ANGLE -30.0f           // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
 
 #define PITCH_MAX_ECD 5100 * ECD_ANGLE_COEF_DJI
 #define PITCH_MIN_ECD 3900 * ECD_ANGLE_COEF_DJI
@@ -146,10 +146,15 @@ typedef struct
     float vy;           // 横移方向速度
     float wz;           // 旋转速度
     float offset_angle; // 底盘和归中位置的夹角
+    float leg_length;   // 腿长
     chassis_mode_e chassis_mode;
     int chassis_speed_buff;
     // UI部分
     //  ...
+    shoot_mode_e shoot_mode;				 // 发射模式设置
+    friction_mode_e friction_mode;			 // 摩擦轮关闭
+
+    uint8_t UI_refresh;
 
 } Chassis_Ctrl_Cmd_s;
 
@@ -189,11 +194,11 @@ typedef struct
 // 后续增加底盘的真实速度
     // float real_vx;
     // float real_vy;
-    // float real_wz;
+    // 底盘旋转速度
+    float real_wz;
 
-    uint8_t rest_heat;           // 剩余枪口热量
-    Bullet_Speed_e bullet_speed; // 弹速限制
     Enemy_Color_e enemy_color;   // 0 for blue, 1 for red
+    uint8_t power_management_chassis_output;
 
 } Chassis_Upload_Data_s;
 

application/robot_task.h

@@ -83,9 +83,9 @@ __attribute__((noreturn)) void StartMOTORTASK(void const *argument)
         motor_start = DWT_GetTimeline_ms();
         MotorControlTask();
         motor_dt = DWT_GetTimeline_ms() - motor_start;
-        if (motor_dt > 1)
+        if (motor_dt > 5)
             LOGERROR("[freeRTOS] MOTOR Task is being DELAY! dt = [%f]", &motor_dt);
-        osDelay(1);
+        osDelay(5);
     }
 }
 
@@ -119,9 +119,9 @@ __attribute__((noreturn)) void StartROBOTTASK(void const *argument)
         robot_start = DWT_GetTimeline_ms();
         RobotTask();
         robot_dt = DWT_GetTimeline_ms() - robot_start;
-        if (robot_dt > 1)
+        if (robot_dt > 2)
             LOGERROR("[freeRTOS] ROBOT core Task is being DELAY! dt = [%f]", &robot_dt);
-        osDelay(1);
+        osDelay(2);
     }
 }
 

application/shoot/shoot.c

@@ -52,10 +52,10 @@ void ShootInit()
             .motor_reverse_flag = MOTOR_DIRECTION_REVERSE,
         },
         .motor_type = M3508};
-    friction_config.can_init_config.tx_id = 1,
+    friction_config.can_init_config.tx_id = 2,
     friction_l = DJIMotorInit(&friction_config);
 
-    friction_config.can_init_config.tx_id = 2; // 右摩擦轮,改txid和方向就行
+    friction_config.can_init_config.tx_id = 1; // 右摩擦轮,改txid和方向就行
     friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
     friction_r = DJIMotorInit(&friction_config);
 
@@ -221,8 +221,8 @@ void ShootTask()
 //            DJIMotorSetRef(friction_r, 0);
 //            break;
         default: // 当前为了调试设定的默认值4000,因为还没有加入裁判系统无法读取弹速.
-            DJIMotorSetRef(friction_l, 30000);
+            DJIMotorSetRef(friction_l, 45000);
-            DJIMotorSetRef(friction_r, 30000);
+            DJIMotorSetRef(friction_r, 45000);
             break;
         }
     }

gimbal.jdebug

@@ -31,6 +31,7 @@ void OnProjectLoad (void) {
   //
   // User settings
   //
+  Edit.SysVar (VAR_HSS_SPEED, FREQ_500_HZ);
   Project.SetOSPlugin ("FreeRTOSPlugin_CM4");
   File.Open ("$(ProjectDir)/cmake-build-debug/wheel_legged_gimbal.elf");
 

gimbal.jdebug.user

@@ -1,34 +1,33 @@
 
 
-GraphedExpression="((vt_data[0]).mouse).x", DisplayFormat=DISPLAY_FORMAT_DEC, Color=#e56a6f
+OpenDocument="robot_cmd.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/application/cmd/robot_cmd.c", Line=6
-OpenDocument="robot_cmd.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/application/cmd/robot_cmd.c", Line=89
 OpenDocument="main.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/Src/main.c", Line=70
 OpenDocument="dji_motor.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/modules/motor/DJImotor/dji_motor.c", Line=137
 OpenDocument="bsp_dwt.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/bsp/dwt/bsp_dwt.c", Line=101
-OpenDocument="tasks.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/Middlewares/Third_Party/FreeRTOS/Source/tasks.c", Line=3414
+OpenDocument="tasks.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/Middlewares/Third_Party/FreeRTOS/Source/tasks.c", Line=3641
 OpenDocument="SEGGER_RTT_printf.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/Middlewares/Third_Party/SEGGER/RTT/SEGGER_RTT_printf.c", Line=108
 OpenDocument="shoot.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/application/shoot/shoot.c", Line=99
 OpenDocument="robot.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/application/robot.c", Line=8
-OpenDocument="master_process.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/modules/master_machine/master_process.c", Line=155
+OpenDocument="master_process.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/modules/master_machine/master_process.c", Line=154
 OpenDocument="stm32f4xx_hal_spi.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_spi.c", Line=1274
-OpenDocument="gimbal.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/application/gimbal/gimbal.c", Line=125
 OpenDocument="startup_stm32f407ighx.s", FilePath="D:/CLion/Project/wheel_legged_gimbal/Startup/startup_stm32f407ighx.s", Line=52
+OpenDocument="gimbal.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/application/gimbal/gimbal.c", Line=192
 OpenDocument="controller.c", FilePath="D:/CLion/Project/wheel_legged_gimbal/modules/algorithm/controller.c", Line=57
 OpenToolbar="Debug", Floating=0, x=0, y=0
-OpenWindow="Registers 1", DockArea=RIGHT, x=0, y=1, w=695, h=394, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, FilteredItems=[], RefreshRate=1
+OpenWindow="Registers 1", DockArea=RIGHT, x=0, y=1, w=695, h=367, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, FilteredItems=[], RefreshRate=1
-OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=569, h=278, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
+OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=569, h=291, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
-OpenWindow="Disassembly", DockArea=RIGHT, x=0, y=0, w=695, h=226, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
+OpenWindow="Disassembly", DockArea=RIGHT, x=0, y=0, w=695, h=253, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
-OpenWindow="Memory 1", DockArea=BOTTOM, x=3, y=0, w=128, h=287, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, EditorAddress=0x0
+OpenWindow="Memory 1", DockArea=BOTTOM, x=3, y=0, w=184, h=287, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, EditorAddress=0x0
-OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=1, w=569, h=358, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
+OpenWindow="Watched Data 1", DockArea=LEFT, x=0, y=1, w=569, h=345, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
-OpenWindow="Terminal", DockArea=BOTTOM, x=0, y=0, w=986, h=287, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
+OpenWindow="Terminal", DockArea=BOTTOM, x=0, y=0, w=950, h=287, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
 OpenWindow="Data Sampling", DockArea=BOTTOM, x=2, y=0, w=264, h=287, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0
-OpenWindow="Timeline", DockArea=RIGHT, x=0, y=1, w=695, h=394, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="2 s / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="0;0", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="313;0", CodeGraphLegendShown=0, CodeGraphLegendPosition="326;0"
+OpenWindow="Timeline", DockArea=RIGHT, x=0, y=1, w=695, h=367, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="2 s / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="0;0", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="329;0", CodeGraphLegendShown=0, CodeGraphLegendPosition="343;0"
-OpenWindow="Console", DockArea=BOTTOM, x=1, y=0, w=539, h=287, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
+OpenWindow="Console", DockArea=BOTTOM, x=1, y=0, w=519, h=287, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
 SmartViewPlugin="", Page="", Toolbar="Hidden", Window="SmartView 1"
 TableHeader="Registers 1", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Description"], ColWidths=[100;118;456]
 TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[233;100;100;100;784]
-TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" ((vt_data[0]).mouse).x"], ColWidths=[100;100;100]
+TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time"], ColWidths=[100;100]
-TableHeader="Data Sampling Setup", SortCol="Type", SortOrder="DESCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[359;100;100;100;100;102;100;107;107]
+TableHeader="Data Sampling Setup", SortCol="Type", SortOrder="DESCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[359;100;100;100;100;100;100;107;107]
 TableHeader="Power Sampling", SortCol="Index", SortOrder="ASCENDING", VisibleCols=["Index";"Time";"Ch 0"], ColWidths=[100;100;100]
 TableHeader="Task List", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Name";"Run Count";"Priority";"Status";"Timeout";"Stack Info";"ID";"Mutex Count";"Notified Value";"Notify State"], ColWidths=[110;110;110;110;110;110;110;110;110;110]
 TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh"], ColWidths=[166;134;100;148]
@@ -50,3 +49,6 @@ WatchedExpression="rc_data", Window=Watched Data 1
 WatchedExpression="gimbal_cmd_send", RefreshRate=2, Window=Watched Data 1
 WatchedExpression="vt_data", RefreshRate=2, Window=Watched Data 1
 WatchedExpression="chassis_cmd_send", RefreshRate=2, Window=Watched Data 1
+WatchedExpression="gimbal_feedback_data", RefreshRate=2, Window=Watched Data 1
+WatchedExpression="(((yaw_motor)->motor_controller).speed_PID).Measure", Window=Watched Data 1
+WatchedExpression="referee_data", RefreshRate=2, Window=Watched Data 1

modules/auto_aim/auto_aim.c

@@ -1,9 +1,4 @@
 //
-// Created by SJQ on 2024/1/26.
-//
-
-#include "auto_aim.h"
-//
 // Created by sph on 2024/1/21.
 //
 #include "auto_aim.h"
@@ -15,7 +10,7 @@
  * @param[in]      trajectory_cal:弹道解算结构体
  * @retval         返回空
  */
-void aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal) {
+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;
 
@@ -35,14 +30,22 @@ void aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_
             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 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);
+//        float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[1].yaw);
-        if (temp_yaw_diff < yaw_diff_min) {
+//        if (temp_yaw_diff < yaw_diff_min) {
-            yaw_diff_min = temp_yaw_diff;
+//            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++) {
@@ -60,15 +63,71 @@ void aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_
             use_1 = !use_1;
         }
 
-        //计算枪管到目标装甲板yaw最小的那个装甲板
+//        //计算枪管到目标装甲板yaw最小的那个装甲板
-        float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].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++) {
-            float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[i].yaw);
+            if (distance[i] < distance[label_first]) {
-            if (temp_yaw_diff < yaw_diff_min) {
+                label_second = label_first;
-                yaw_diff_min = temp_yaw_diff;
+                label_first = i;
-                idx = i;
+            }  else if (distance[i] < distance[label_second] && distance[i] < distance[label_first]) {
+                label_second = i;
+            }else if (distance[i] < distance[label_second]) {
+                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;
@@ -82,19 +141,70 @@ void aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_
             use_1 = !use_1;
         }
 
-        //计算枪管到目标装甲板yaw最小的那个装甲板
+//        //计算枪管到目标装甲板yaw最小的那个装甲板
-        float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw);
+//        float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw);
-        for (i = 1; i < 4; i++) {
+//        for (i = 1; i < 4; i++) {
-            float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[i].yaw);
+//            float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[i].yaw);
-            if (temp_yaw_diff < yaw_diff_min) {
+//            if (temp_yaw_diff < yaw_diff_min) {
-                yaw_diff_min = temp_yaw_diff;
+//                yaw_diff_min = temp_yaw_diff;
-                idx = i;
+//                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 = 1;
+
+        if(distance[label_first] > distance[label_second])
+        {
+            label_first = 1;
+            label_second = 0;
         }
+
+        if(distance[2]<distance[label_first]){
+            label_second = label_first;
+            label_first = 2;
         }
+        else if(distance[2]<distance[label_second])
+            label_second = 2;
+
+
+        if(distance[3]<distance[label_first]){
+            label_second = label_first;
+            label_first = 3;
+        }
+        else if(distance[3]<distance[label_second])
+            label_second = 3;
+
+        //再选择两块装甲板与自身位置连线，同旋转中心自身位置连线，夹角较小的为目标装甲板
+        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;
+    }
+
+    idx = 0;
+
     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;
 }
 
 /**
@@ -105,7 +215,7 @@ void aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_
   * @param[in]      v_x0:弹速水平分量
   * @retval         返回空
   */
-const float k1 = 0.0761; //标准大气压25度下空气阻力系数（小弹丸）
+const float k1 = 0.0761; //标准大气压25度下空气阻力系数
 float get_fly_time(float x, float vx, float v_x0) {
     float t = 0;
     float f_ti = 0, df_ti = 0;
@@ -162,8 +272,8 @@ void get_cmd_angle(Trajectory_Type_t *trajectory_cal) {
     trajectory_cal->cmd_pitch = trajectory_cal->theta_k;
 }
 
-void auto_aim(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal, RecievePacket_t *receive_packet) {
+int auto_aim(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal, RecievePacket_t *receive_packet) {
-    trajectory_cal->extra_delay_time = 0.025;//0.025
+    trajectory_cal->extra_delay_time = 0.035;//0.025
 
     aim_sel->target_state.armor_type = receive_packet->id;
     aim_sel->target_state.armor_num = receive_packet->armors_num;
@@ -179,7 +289,7 @@ void auto_aim(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal, Rec
     aim_sel->target_state.r2 = receive_packet->r2;
     aim_sel->target_state.dz = receive_packet->dz;
 
-    aim_armor_select(aim_sel, trajectory_cal);
+    int 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];
@@ -189,5 +299,5 @@ void auto_aim(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal, Rec
     trajectory_cal->velocity[2] = aim_sel->target_state.vz;
 
     get_cmd_angle(trajectory_cal);
-
+    return idx;
 }

modules/auto_aim/auto_aim.h

@@ -1,14 +1,10 @@
-//
-// Created by SJQ on 2024/1/26.
-//
-
-#ifndef WHEEL_LEGGED_GIMBAL_AUTO_AIM_H
-#define WHEEL_LEGGED_GIMBAL_AUTO_AIM_H
-
 //
 // Created by sph on 2024/1/21.
 //
 
+#ifndef BASIC_FRAMEWORK_AUTO_AIM_H
+#define BASIC_FRAMEWORK_AUTO_AIM_H
+
 #include "master_process.h"
 //弹道解算
 typedef  struct
@@ -73,11 +69,9 @@ typedef struct
 }Aim_Select_Type_t;
 
 
-void aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal);
+int aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal);
 float get_fly_time(float x, float vx, float v_x0);
 void get_cmd_angle(Trajectory_Type_t *trajectory_cal);
-void auto_aim(Aim_Select_Type_t *aim_sel,Trajectory_Type_t *trajectory_cal,RecievePacket_t *receive_packet);
+int auto_aim(Aim_Select_Type_t *aim_sel,Trajectory_Type_t *trajectory_cal,RecievePacket_t *receive_packet);
 
-
+#endif //BASIC_FRAMEWORK_AUTO_AIM_H
-
-#endif //WHEEL_LEGGED_GIMBAL_AUTO_AIM_H

modules/master_machine/master_process.c

@@ -177,8 +177,7 @@ void VisionSend()
     static uint8_t send_buffer[24]={0};
 
     send_data.header = 0x5A;
-    //VisionSetFlag(1);
+
-    VisionSetAim(recv_data.x,recv_data.y,recv_data.z);
     send_data.checksum = crc_16(&send_data.header,sizeof(send_data)-2);
 
     memcpy(send_buffer,&send_data,sizeof(send_data));