烧饼自瞄

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;

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;

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, 
+            .p = 7.0f, 
-            .i = 0.0f, 
+            .i = 0.5f, 
-            .d = 0.0f, 
+            .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,
+                .k=0.7f,
-                .p=0.5f,
+                .p=0.7f,
                 .i=0.5f,
                 .d=0.05f,
                 .i_limit=0.2f,

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

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); /* 运行结束，等待下一次唤醒 */
   }

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); /* 运行结束，等待下一次唤醒 */
   }

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(); // 解锁内核

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

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;