From 72a4e5b073a36212195ce7b889fcf5b7698b31bd Mon Sep 17 00:00:00 2001 From: Xiaocheng <2544262366@qq.com> Date: Tue, 13 Jan 2026 11:11:19 +0800 Subject: [PATCH] =?UTF-8?q?=E7=83=A7=E9=A5=BC=E8=87=AA=E7=9E=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/cmd.c | 5 +++ User/module/cmd.h | 6 ++++ User/module/config.c | 30 ++++++++++++++---- User/module/gimbal.c | 2 +- User/task/Task8.c | 74 +++++++++++++++++++++++++++++++++++++++++++ User/task/ai.c | 3 ++ User/task/init.c | 4 +-- User/task/shoot.c | 23 ++++++++++++-- User/task/user_task.h | 1 + 9 files changed, 137 insertions(+), 11 deletions(-) diff --git a/User/module/cmd.c b/User/module/cmd.c index fcf0275..9ad13c7 100644 --- a/User/module/cmd.c +++ b/User/module/cmd.c @@ -146,15 +146,19 @@ int8_t CMD_CtrlSet(Chassis_CMD_t *c_cmd, const CMD_RC_t *rc, Gimbal_CMD_t *g_cmd { case CMD_SW_UP: g_cmd->ctrl_mode = GIMBAL_MODE_REMOTE; + s_cmd->control_mode=SHOOT_REMOTE; break; case CMD_SW_MID: g_cmd->ctrl_mode = GIMBAL_MODE_AI; + s_cmd->control_mode=SHOOT_AI; break; case CMD_SW_DOWN: g_cmd->ctrl_mode = GIMBAL_MODE_AI; + s_cmd->control_mode=SHOOT_AI; break; case CMD_SW_ERR: g_cmd->ctrl_mode = GIMBAL_MODE_REMOTE; + s_cmd->control_mode=SHOOT_REMOTE; break; } @@ -191,6 +195,7 @@ int8_t CMD_CtrlSet(Chassis_CMD_t *c_cmd, const CMD_RC_t *rc, Gimbal_CMD_t *g_cmd } switch(rc->ET16s.key_D) { + case CMD_SW_UP: s_cmd->firecmd = false; s_cmd->ready = false; diff --git a/User/module/cmd.h b/User/module/cmd.h index 136d450..327fcef 100644 --- a/User/module/cmd.h +++ b/User/module/cmd.h @@ -126,8 +126,14 @@ typedef enum { SHOOT_MODE_NUM }Shoot_Mode_t; +typedef enum{ + SHOOT_REMOTE, + SHOOT_AI +}Shoot_CONTROL_Mode_t; + typedef struct { Shoot_Mode_t mode;/* 射击模式 */ + Shoot_CONTROL_Mode_t control_mode; bool ready; /* 准备射击 */ bool firecmd; /* 射击 */ }Shoot_CMD_t; diff --git a/User/module/config.c b/User/module/config.c index fbf5c16..6093538 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -165,10 +165,19 @@ static const Config_Param_t config = { // .d_cutoff_freq = 0.0f, // .range = M_2PI /*上面一组pid会导致电机超调，可能是因为pid计算错误，正在进行重新修改pid*/ + // .k = 2.0f, + // .p = 5.0f, + // .i = 0.0f, + // .d = 0.0f, + // .i_limit = 1.0f, + // .out_limit = 10.0f, + // .d_cutoff_freq = 0.0f, + // .range = M_2PI + .k = 2.0f, - .p = 5.0f, - .i = 0.0f, - .d = 0.0f, + .p = 7.0f, + .i = 0.5f, + .d = 1.0f, .i_limit = 1.0f, .out_limit = 10.0f, .d_cutoff_freq = 0.0f, @@ -185,8 +194,17 @@ static const Config_Param_t config = { // .d_cutoff_freq = -1.0f, // .range = -1.0f + // .k = 0.35f, + // .p = 0.3f, + // .i = 0.0f, + // .d = 0.0f, + // .i_limit = 1.0f, + // .out_limit = 1.0f, + // .d_cutoff_freq = -1.0f, + // .range = -1.0f + .k = 0.35f, - .p = 0.3f, + .p = 0.35f, .i = 0.0f, .d = 0.0f, .i_limit = 1.0f, @@ -253,8 +271,8 @@ static const Config_Param_t config = { }, .trig_2006={ - .k=0.5f, - .p=0.5f, + .k=0.7f, + .p=0.7f, .i=0.5f, .d=0.05f, .i_limit=0.2f, diff --git a/User/module/gimbal.c b/User/module/gimbal.c index 230f9e9..13b4d85 100644 --- a/User/module/gimbal.c +++ b/User/module/gimbal.c @@ -377,7 +377,7 @@ void Gimbal_Output(Gimbal_t *g) MOTOR_MIT_Output_t output_yaw_4310 = {0}; output_yaw_4310.torque = g->out.yaw_4310 * 1.0f; - output_yaw_4310.kd = 0.1f; + output_yaw_4310.kd = 0.2f; // output_yaw_4310.kd=0.1f; MOTOR_MIT_Output_t output_pitch_4310 = {0}; diff --git a/User/task/Task8.c b/User/task/Task8.c index 17446e5..13b07b5 100644 --- a/User/task/Task8.c +++ b/User/task/Task8.c @@ -6,6 +6,9 @@ /* Includes ----------------------------------------------------------------- */ #include "task/user_task.h" /* USER INCLUDE BEGIN */ +#include "bsp/can.h" +#include "device/motor_rm.h" +#include "component/pid.h" /* USER INCLUDE END */ @@ -14,7 +17,57 @@ /* Private macro ------------------------------------------------------------ */ /* Private variables -------------------------------------------------------- */ /* USER STRUCT BEGIN */ +MOTOR_RM_t motor_6020; +MOTOR_RM_t motor_3508; +MOTOR_RM_Param_t motor_6020_param = { + .can = BSP_CAN_1, + .id = 0x205, + .module = MOTOR_GM6020, + .reverse = false, + .gear = false, +}; +MOTOR_RM_Param_t motor_3508_param = { + .can = BSP_CAN_1, + .id = 0x201, + .module = MOTOR_M3508, + .reverse = false, + .gear = false, +}; +KPID_t motor_6020_speed; +KPID_t motor_3508_angle; +KPID_t motor_3508_omega; + +KPID_Params_t motor_6020_speed_param = { + .k = 0.1f, + .p = 0.2f, + .i = 0.1f, + .d = 0.0f, + .i_limit = 1.0f, + .out_limit = 1.0f, + .d_cutoff_freq = 30.0f, +}; +KPID_Params_t motor_3508_angle_param = { + .k = 1.0f, + .p = 5.0f, + .i = 0.1f, + .d = 0.0f, + .i_limit = 1.0f, + .out_limit = 1.0f, + .d_cutoff_freq = 30.0f, +}; +KPID_Params_t motor_3508_omega_param = { + .k = 1.0f, + .p = 5.0f, + .i = 0.1f, + .d = 0.0f, + .i_limit = 1.0f, + .out_limit = 1.0f, + .d_cutoff_freq = 30.0f, +}; + +float motor_3508_target_angle = 5*M_2PI; +float motor_6020_target_speed = 33.0f; /* USER STRUCT END */ /* Private function --------------------------------------------------------- */ @@ -30,13 +83,34 @@ void Task_Task8(void *argument) { uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ /* USER CODE INIT BEGIN */ + BSP_CAN_Init(); +MOTOR_RM_Register( &motor_6020_param); +MOTOR_RM_Register(&motor_3508_param); +PID_Init(&motor_6020_speed, KPID_MODE_CALC_D, TASK8_FREQ, &motor_6020_speed_param); +PID_Init(&motor_3508_angle, KPID_MODE_NO_D, TASK8_FREQ, &motor_3508_angle_param); +PID_Init(&motor_3508_omega, KPID_MODE_NO_D, TASK8_FREQ, &motor_3508_omega_param); /* USER CODE INIT END */ while (1) { tick += delay_tick; /* 计算下一个唤醒时刻 */ /* USER CODE BEGIN */ + MOTOR_RM_UpdateAll(); + motor_6020.feedback.rotor_speed= (MOTOR_GetRotorSpeed(&(MOTOR_RM_GetMotor(&motor_6020_param)->motor)) ); + motor_3508.feedback.rotor_abs_angle= MOTOR_RM_GetMotor(&motor_3508_param)->gearbox_total_angle; + motor_3508.feedback.rotor_speed= (MOTOR_GetRotorSpeed(&(MOTOR_RM_GetMotor(&motor_3508_param)->motor)) ); + + + float motor_6020out,motor_3508_ommega,motor_3508_out; + + motor_6020out = PID_Calc(&motor_6020_speed, motor_6020_target_speed, motor_6020.feedback.rotor_speed, 0.0f,0.0f); + + // motor_3508_ommega = PID_Calc(&motor_3508_omega, motor_3508_target_angle, motor_3508.feedback.rotor_abs_angle, 0.0f,0.0f); + // motor_3508_out = PID_Calc(&motor_3508_angle, motor_3508_ommega, motor_3508.feedback.rotor_speed, 0.0f,0.0f); + + MOTOR_RM_SetOutput(&motor_6020_param, motor_6020out); + MOTOR_RM_Ctrl(&motor_6020_param); /* USER CODE END */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ } diff --git a/User/task/ai.c b/User/task/ai.c index 545cef9..1a7cff4 100644 --- a/User/task/ai.c +++ b/User/task/ai.c @@ -49,6 +49,9 @@ void Task_ai(void *argument) { AI_Get_NUC(&ai,&ai_cmd); osMessageQueueReset(task_runtime.msgq.gimbal.ai.g_cmd); osMessageQueuePut(task_runtime.msgq.gimbal.ai.g_cmd, &ai_cmd, 0, 0); + + osMessageQueueReset(task_runtime.msgq.gimbal.ai.s_cmd); + osMessageQueuePut(task_runtime.msgq.gimbal.ai.s_cmd, &ai_cmd, 0, 0); /* USER CODE END */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ } diff --git a/User/task/init.c b/User/task/init.c index b702be9..53daf62 100644 --- a/User/task/init.c +++ b/User/task/init.c @@ -38,7 +38,7 @@ void Task_Init(void *argument) { task_runtime.thread.gimbal = osThreadNew(Task_gimbal, NULL, &attr_gimbal); task_runtime.thread.shoot = osThreadNew(Task_shoot, NULL, &attr_shoot); task_runtime.thread.ai = osThreadNew(Task_ai, NULL, &attr_ai); - task_runtime.thread.Task8 = osThreadNew(Task_Task8, NULL, &attr_Task8); + // task_runtime.thread.Task8 = osThreadNew(Task_Task8, NULL, &attr_Task8); // 创建消息队列 /* USER MESSAGE BEGIN */ @@ -54,7 +54,7 @@ void Task_Init(void *argument) { task_runtime.msgq.shoot.cmd = osMessageQueueNew(2u, sizeof(Shoot_CMD_t),NULL); task_runtime.msgq.gimbal.ai.feedback = osMessageQueueNew(2u, sizeof(Gimbal_feedback_t),NULL); task_runtime.msgq.gimbal.ai.g_cmd = osMessageQueueNew(2u, sizeof(AI_cmd_t),NULL); - + task_runtime.msgq.gimbal.ai.s_cmd = osMessageQueueNew(2u, sizeof(AI_cmd_t),NULL); /* USER MESSAGE END */ osKernelUnlock(); // 解锁内核 diff --git a/User/task/shoot.c b/User/task/shoot.c index 3565cb0..bd873e7 100644 --- a/User/task/shoot.c +++ b/User/task/shoot.c @@ -8,7 +8,7 @@ /* USER INCLUDE BEGIN */ #include "module/shoot.h" #include "module/config.h" - +#include "device/ai.h" /* USER INCLUDE END */ /* Private typedef ---------------------------------------------------------- */ @@ -18,6 +18,7 @@ /* USER STRUCT BEGIN */ Shoot_t shoot; Shoot_CMD_t shoot_cmd; +AI_cmd_t shoot_ai_cmd; /* USER STRUCT END */ /* Private function --------------------------------------------------------- */ @@ -42,8 +43,26 @@ Shoot_SetMode(&shoot,SHOOT_MODE_SINGLE); tick += delay_tick; /* 计算下一个唤醒时刻 */ /* USER CODE BEGIN */ osMessageQueueGet(task_runtime.msgq.shoot.cmd, &shoot_cmd, NULL, 0); - +osMessageQueueGet(task_runtime.msgq.gimbal.ai.s_cmd, &shoot_ai_cmd, NULL, 0); +if(shoot_cmd.control_mode==SHOOT_REMOTE) +{ + //do nothing，使用遥控器的指令 +} +else if(shoot_cmd.control_mode==SHOOT_AI) +{ +if(shoot_ai_cmd.mode==0 || shoot_ai_cmd.mode==1) +{ + shoot_cmd.firecmd=false; + shoot_cmd.ready=true; + +} +else if(shoot_ai_cmd.mode==2) +{ + shoot_cmd.firecmd=true; + shoot_cmd.ready=true; +} + } Shoot_UpdateFeedback(&shoot); Shoot_SetMode(&shoot,shoot_cmd.mode); diff --git a/User/task/user_task.h b/User/task/user_task.h index 3e33264..f836d7e 100644 --- a/User/task/user_task.h +++ b/User/task/user_task.h @@ -91,6 +91,7 @@ typedef struct { osMessageQueueId_t quat; osMessageQueueId_t feedback; osMessageQueueId_t g_cmd; + osMessageQueueId_t s_cmd; }ai; /* 新增的 ai 消息队列 主要是给自瞄*/ } gimbal;