更新shoot

User/device/motor_dm.h

@@ -43,7 +43,7 @@ typedef struct {
 /* Exported functions prototypes -------------------------------------------- */
 
 /**
- * @brief 注册一个LK电机
+ * @brief 注册一个DM电机
  * @param param 电机参数
  * @return 
  */
@@ -62,11 +62,31 @@ int8_t MOTOR_DM_Update(MOTOR_DM_Param_t *param);
  */
 int8_t MOTOR_DM_UpdateAll(void);
 
+/**
+ * @brief MIT模式控制电机
+ * @param param 电机参数
+ * @param output MIT控制输出参数
+ * @return 操作结果，0为成功，负值为失败
+ */
 int8_t MOTOR_DM_MITCtrl(MOTOR_DM_Param_t *param, MOTOR_MIT_Output_t *output);
 
+/**
+ * @brief 位置速度双环控制电机
+ * @param param 电机参数
+ * @param target_pos 目标位置
+ * @param target_vel 目标速度
+ * @return 操作结果，0为成功，负值为失败
+ */
 int8_t MOTOR_DM_PosVelCtrl(MOTOR_DM_Param_t *param, float target_pos, float target_vel);
 
-int8_t MOTOR_DM_VelCtrl(MOTOR_DM_Param_t *param, float target_vel);
+/**
+ * @brief 位置速度双环控制电机
+ * @param param 电机参数
+ * @param target_pos 目标位置
+ * @param target_vel 目标速度
+ * @return 操作结果，0为成功，负值为失败
+ */
+int8_t MOTOR_DM_PosVelCtrl(MOTOR_DM_Param_t *param, float target_pos, float target_vel);
 
 /**
  * @brief 获取指定电机的实例指针
@@ -75,6 +95,11 @@ int8_t MOTOR_DM_VelCtrl(MOTOR_DM_Param_t *param, float target_vel);
  */
 MOTOR_DM_t* MOTOR_DM_GetMotor(MOTOR_DM_Param_t *param);
 
+/**
+ * @brief 使能电机
+ * @param param 电机参数
+ * @return 操作结果，0为成功，负值为失败
+ */
 int8_t MOTOR_DM_Enable(MOTOR_DM_Param_t *param);
 
 /**

User/device/motor_rm.c

@@ -139,13 +139,13 @@ static void Motor_RM_Decode(MOTOR_RM_t *motor, BSP_CAN_Message_t *msg) {
         motor->feedback.rotor_speed = rotor_speed;
         motor->feedback.torque_current = torque_current;
     }
-    if (motor->motor.reverse) {
     while (motor->feedback.rotor_abs_angle < 0) {
         motor->feedback.rotor_abs_angle += M_2PI;
     }
     while (motor->feedback.rotor_abs_angle >= M_2PI) {
         motor->feedback.rotor_abs_angle -= M_2PI;
     }
+    if (motor->motor.reverse) {
         motor->feedback.rotor_abs_angle = M_2PI - motor->feedback.rotor_abs_angle;
         motor->feedback.rotor_speed = -motor->feedback.rotor_speed;
         motor->feedback.torque_current = -motor->feedback.torque_current;

User/module/config.c

@@ -23,6 +23,8 @@
 Config_RobotParam_t robot_config = {
     .shoot_param = {
             .trig_step_angle=M_2PI/8,
+            .shot_delay_time=0.05f,
+            .shot_burst_num=1,
             .fric_motor_param[0] = {
                 .can = BSP_CAN_2,
                 .id = 0x201,
@@ -41,7 +43,7 @@ Config_RobotParam_t robot_config = {
                 .can = BSP_CAN_1,
                 .id = 0x201,
                 .module = MOTOR_M2006,
-                .reverse = false,
+                .reverse = true,
                 .gear=true,
             },
             .fric_follow = {
@@ -54,6 +56,7 @@ Config_RobotParam_t robot_config = {
                 .d_cutoff_freq=30.0f,
                 .range=-1.0f,
             },
+            
             .fric_err = {
                 .k=1.0f,
                 .p=4.0f,
@@ -64,16 +67,26 @@ Config_RobotParam_t robot_config = {
                 .d_cutoff_freq=40.0f,
                 .range=-1.0f, 
             },
-            .trig = {
+            .trig_omg = {
             .k=1.0f,
-                .p=1.2f,
+            .p=1.5f,
-                .i=0.0f,
+            .i=0.3f,
-                .d=0.05f,
+            .d=0.5f,
             .i_limit=0.2f,
             .out_limit=0.9f,
-                .d_cutoff_freq=30.0f,
+            .d_cutoff_freq=-1.0f,
             .range=-1.0f,
             },
+            .trig = {
+                .k=1.0f,
+                .p=1.0f,
+                .i=0.1f,
+                .d=0.05f,
+                .i_limit=0.8f,
+                .out_limit=0.5f,
+                .d_cutoff_freq=-1.0f,
+                .range=M_2PI,
+            },
             .filter.fric = {
                 .in = 30.0f,
                 .out = 30.0f,
@@ -88,7 +101,7 @@ Config_RobotParam_t robot_config = {
     .gimbal_param = {
         .pid = {
             .yaw_omega = {
-                .k = 0.5f,
+                .k = 1.0f,
                 .p = 1.0f,
                 .i = 0.5f,
                 .d = 0.0f,
@@ -101,7 +114,7 @@ Config_RobotParam_t robot_config = {
                 .k = 10.0f,
                 .p = 3.0f,
                 .i = 0.0f,
-                .d = 0.1f,
+                .d = 0.0f,
                 .i_limit = 0.0f,
                 .out_limit = 10.0f,
                 .d_cutoff_freq = -1.0f,
@@ -130,7 +143,7 @@ Config_RobotParam_t robot_config = {
         },
         .mech_zero = {
             .yaw = 0.0f,
-            .pit = 3.8f,
+            .pit = 1.77f,
         },
         .travel = {
             .yaw = -1.0f,

User/module/gimbal.c

@@ -159,7 +159,7 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
   Gimbal_SetMode(g, g_cmd->mode);
 
   /* 处理yaw控制命令，软件限位 - 使用电机绝对角度 */
-  float delta_yaw = g_cmd->delta_yaw * g->dt;
+  float delta_yaw = g_cmd->delta_yaw * g->dt * 1.5f;
   if (g->param->travel.yaw > 0) {
     /* 计算当前电机角度与IMU角度的偏差 */
     float motor_imu_offset = g->feedback.motor.yaw.rotor_abs_angle - g->feedback.imu.eulr.yaw;
@@ -239,7 +239,8 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
 void Gimbal_Output(Gimbal_t *g){
     MOTOR_RM_SetOutput(&g->param->pit_motor, g->out.pit);
     MOTOR_MIT_Output_t output = {0};
-    output.torque = g->out.yaw * 2.5;
+    output.torque = g->out.yaw * 6.0f; // 乘以减速比
+    output.kd = 0.5f;
     MOTOR_RM_Ctrl(&g->param->pit_motor);
     MOTOR_DM_MITCtrl(&g->param->yaw_motor, &output);
   }

User/module/shoot.c

@@ -1,16 +1,13 @@
 
 #include "shoot.h"
-
-#include "bsp/can.h"
-#include "component/filter.h"
 #include <string.h>
-// #include
+#include "can.h"
+#include "component/filter.h"
+#include "component/user_math.h"
 #include <math.h>
 #include "bsp/time.h"
 
-uint32_t shoot_ctrl_cnt_last;
+static bool last_firecmd;
-float shoot_ctrl_dt;
-bool last_firecmd=false;
 
 static inline void ScaleSumTo1(float *a, float *b) {
     float sum = *a + *b;
@@ -21,149 +18,281 @@ static inline void ScaleSumTo1(float *a, float *b) {
     }
 }
 
-int8_t Shoot_Init(Shoot_t *c, Shoot_Params_t *param, float target_freq)
+
+int8_t Shoot_SetMode(Shoot_t *s, Shoot_Mode_t mode)
 {
-    if (c == NULL || param == NULL || target_freq <= 0.0f) {
+    if (s == NULL) {
         return -1; // 参数错误
     }
-    c->param=param;
+    s->mode=mode;
-    BSP_CAN_Init();
-    memset(&c->feedback, 0, sizeof(c->feedback));
-
-    for(int i=0;i<SHOOT_FRIC_NUM;i++){
-	MOTOR_RM_Register(&param->fric_motor_param[i]);
-    PID_Init(&c->pid.fric_follow[i], KPID_MODE_CALC_D, target_freq,&param->fric_follow);
-    LowPassFilter2p_Init(&c->filter.fric.in[i], target_freq, c->param->filter.fric.in);
-    LowPassFilter2p_Init(&c->filter.fric.out[i], target_freq, c->param->filter.fric.out);
-    }
-	MOTOR_RM_Register(&param->trig_motor_param);
-    PID_Init(&c->pid.trig, KPID_MODE_CALC_D, target_freq,&param->trig);
-    
-    LowPassFilter2p_Init(&c->filter.trig.in, target_freq, c->param->filter.trig.in);
-    LowPassFilter2p_Init(&c->filter.trig.out, target_freq, c->param->filter.trig.out);
-	
     return 0;
 }
 
-int8_t Chassis_UpdateFeedback(Shoot_t *c)
+int8_t Shoot_ResetIntegral(Shoot_t *s)
 {
-    if (c == NULL) {
+    if (s == NULL) {
         return -1; // 参数错误
     }
-
-    float rpm_sum=0.0f;
-    for(int i = 0; i < SHOOT_FRIC_NUM; i++) {
-        MOTOR_RM_Update(&c->param->fric_motor_param[i]);
-		MOTOR_RM_t *motor_fed = MOTOR_RM_GetMotor(&c->param->fric_motor_param[i]);
-		if(motor_fed!=NULL)
-		{
-			c->feedback.fric[i]=motor_fed->motor.feedback;
-		}
-        c->feedback.fil_fric_rpm[i] = LowPassFilter2p_Apply(&c->filter.fric.in[i], c->feedback.fric[i].rotor_speed);
-        c->feedback.fric_rpm[i] = c->feedback.fil_fric_rpm[i] / MAX_FRIC_RPM;
-        if(c->feedback.fric_rpm[i]>1.0f)c->feedback.fric_rpm[i]=1.0f;
-        if(c->feedback.fric_rpm[i]<-1.0f)c->feedback.fric_rpm[i]=-1.0f;
-        rpm_sum+=c->feedback.fric_rpm[i];
-    }
-    c->feedback.fric_avgrpm=rpm_sum/SHOOT_FRIC_NUM;
-    MOTOR_RM_Update(&c->param->trig_motor_param);
-	c->feedback.trig = MOTOR_RM_GetMotor(&c->param->trig_motor_param);
-    //
-    c->feedback.fil_trig_rpm = LowPassFilter2p_Apply(&c->filter.trig.in, c->feedback.trig->feedback.rotor_speed);
-    c->feedback.trig_rpm = c->feedback.trig->feedback.rotor_speed / MAX_TRIG_RPM;
-    if(c->feedback.trig_rpm>1.0f)c->feedback.trig_rpm=1.0f;  //如果单环效果好就删
-    if(c->feedback.trig_rpm<-1.0f)c->feedback.trig_rpm=-1.0f;
-    //
-    c->errtosee = c->feedback.fric[0].rotor_speed - c->feedback.fric[1].rotor_speed;
-	return 0;
-}
-
-int8_t Shoot_Control(Shoot_t *c, const Shoot_CMD_t *cmd)
-{
-    if (c == NULL || cmd == NULL) {
-        return -1; // 参数错误
-    }
-    c->dt = (BSP_TIME_Get_us() - c->lask_wakeup) / 1000000.0f;
-    c->lask_wakeup = BSP_TIME_Get_us();
-	c->online = cmd->online;
-    if(!c->online /*|| c->mode==SHOOT_MODE_SAFE*/){
     for(int i=0;i<SHOOT_FRIC_NUM;i++)
     {	
-            MOTOR_RM_Relax(&c->param->fric_motor_param[i]);
+        PID_ResetIntegral(&s->pid.fric_follow[i]);
+        PID_ResetIntegral(&s->pid.fric_err[i]);
     }
-        MOTOR_RM_Relax(&c->param->trig_motor_param);
+    PID_ResetIntegral(&s->pid.trig);
+    PID_ResetIntegral(&s->pid.trig_omg);
+    return 0;
+}
+
+int8_t Shoot_ResetCalu(Shoot_t *s)
+{
+    if (s == NULL) {
+        return -1; // 参数错误
+    }
+    for(int i=0;i<SHOOT_FRIC_NUM;i++)
+    {	
+        PID_Reset(&s->pid.fric_follow[i]);
+        PID_Reset(&s->pid.fric_err[i]);
+        LowPassFilter2p_Reset(&s->filter.fric.in[i], 0.0f);
+        LowPassFilter2p_Reset(&s->filter.fric.out[i], 0.0f);
+    }
+    PID_Reset(&s->pid.trig);
+    PID_Reset(&s->pid.trig_omg);
+    LowPassFilter2p_Reset(&s->filter.trig.in, 0.0f);
+    LowPassFilter2p_Reset(&s->filter.trig.out, 0.0f);
+    return 0;
+}
+
+int8_t Shoot_ResetOutput(Shoot_t *s)
+{
+    if (s == NULL) {
+        return -1; // 参数错误
+    }
+    for(int i=0;i<SHOOT_FRIC_NUM;i++)
+    {	
+        s->output.out_follow[i]=0.0f;
+        s->output.out_err[i]=0.0f;
+        s->output.out_fric[i]=0.0f;
+        s->output.lpfout_fric[i]=0.0f;
+    }
+	s->output.outagl_trig=0.0f;
+	s->output.outomg_trig=0.0f;
+    s->output.outlpf_trig=0.0f;
+    return 0;
+}
+
+int8_t Shoot_CaluTargetRPM(Shoot_t *s, float target_speed)
+{
+    if (s == NULL) {
+        return -1; // 参数错误
+    }
+    s->target_variable.target_rpm=4000.0f/MAX_FRIC_RPM;
+    return 0;
+}
+/**
+ * \brief 根据发射弹丸数量及发射频率计算拨弹电机目标角度
+ *
+ * \param s 包含发射数据的结构体
+ * \param num 需要发射的弹丸数量
+ */
+int8_t Shoot_CaluTargetAngle(Shoot_t *s, Shoot_CMD_t *cmd)
+{
+    if (s == NULL  || s->shoot_Anglecalu.num_to_shoot == 0) {
+        return -1; 
+    }
+    if(s->now - s->shoot_Anglecalu.time_last_shoot >= s->param->shot_delay_time && cmd->firecmd)
+    {
+        s->shoot_Anglecalu.time_last_shoot=s->now;
+        s->target_variable.target_angle += s->param->trig_step_angle;
+		if(s->target_variable.target_angle>M_PI)s->target_variable.target_angle-=M_2PI;
+		else if((s->target_variable.target_angle<-M_PI))s->target_variable.target_angle+=M_2PI;
+		s->shoot_Anglecalu.num_to_shoot--;
+    }
+    return 0;
+}
+
+int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq)
+{
+    if (s == NULL || param == NULL || target_freq <= 0.0f) {
+        return -1; // 参数错误
+    }
+    s->param=param;
+
+    BSP_CAN_Init();
+    for(int i=0;i<SHOOT_FRIC_NUM;i++){
+	MOTOR_RM_Register(&param->fric_motor_param[i]);
+    PID_Init(&s->pid.fric_follow[i], KPID_MODE_CALC_D, target_freq,&param->fric_follow);
+    LowPassFilter2p_Init(&s->filter.fric.in[i], target_freq, s->param->filter.fric.in);
+    LowPassFilter2p_Init(&s->filter.fric.out[i], target_freq, s->param->filter.fric.out);
+    }
+	MOTOR_RM_Register(&param->trig_motor_param);
+    PID_Init(&s->pid.trig, KPID_MODE_CALC_D, target_freq,&param->trig);
+    PID_Init(&s->pid.trig_omg, KPID_MODE_CALC_D, target_freq,&param->trig_omg);
+
+    LowPassFilter2p_Init(&s->filter.trig.in, target_freq, s->param->filter.trig.in);
+    LowPassFilter2p_Init(&s->filter.trig.out, target_freq, s->param->filter.trig.out);
+	
+    memset(&s->shoot_Anglecalu,0,sizeof(s->shoot_Anglecalu));
+    memset(&s->output,0,sizeof(s->output));
+    s->target_variable.target_rpm=4000.0f;
+
+	return 0;
+}
+
+int8_t Chassis_UpdateFeedback(Shoot_t *s)
+{
+    if (s == NULL) {
+        return -1; // 参数错误
+    }
+    float rpm_sum=0.0f;
+    for(int i = 0; i < SHOOT_FRIC_NUM; i++) {
+        /* 更新摩擦电机反馈 */
+        MOTOR_RM_Update(&s->param->fric_motor_param[i]);
+		MOTOR_RM_t *motor_fed = MOTOR_RM_GetMotor(&s->param->fric_motor_param[i]);
+		if(motor_fed!=NULL)
+		{
+			s->feedback.fric[i]=motor_fed->motor.feedback;
+		}
+        /* 滤波反馈rpm */
+        s->feedback.fil_fric_rpm[i] = LowPassFilter2p_Apply(&s->filter.fric.in[i], s->feedback.fric[i].rotor_speed);
+        /* 归一化rpm */
+        s->feedback.fric_rpm[i] = s->feedback.fil_fric_rpm[i] / MAX_FRIC_RPM;
+        if(s->feedback.fric_rpm[i]>1.0f)s->feedback.fric_rpm[i]=1.0f;
+        if(s->feedback.fric_rpm[i]<-1.0f)s->feedback.fric_rpm[i]=-1.0f;
+        /* 计算平均rpm */
+        rpm_sum+=s->feedback.fric_rpm[i];
+    }
+    s->feedback.fric_avgrpm=rpm_sum/SHOOT_FRIC_NUM;
+	/* 更新拨弹电机反馈 */
+    MOTOR_RM_Update(&s->param->trig_motor_param);
+	MOTOR_RM_t *motor_fed = MOTOR_RM_GetMotor(&s->param->trig_motor_param);
+		if(motor_fed!=NULL)
+		{
+			s->feedback.trig=motor_fed->feedback;
+		}
+    /* 将多圈角度归化到单圈 (-M_PI, M_PI) */
+		s->feedback.trig_angle_cicle = s->feedback.trig.rotor_abs_angle;
+		s->feedback.trig_angle_cicle = fmod(s->feedback.trig_angle_cicle, M_2PI); // 将角度限制在 [-2π, 2π]
+		if (s->feedback.trig_angle_cicle > M_PI) {
+		s->feedback.trig_angle_cicle -= M_2PI; // 调整到 [-π, π]
+		}else if (s->feedback.trig_angle_cicle < -M_PI) {
+		s->feedback.trig_angle_cicle += M_2PI; // 调整到 [-π, π]
+		}
+    
+    s->feedback.fil_trig_rpm = LowPassFilter2p_Apply(&s->filter.trig.in, s->feedback.trig.rotor_speed);
+    s->feedback.trig_rpm = s->feedback.trig.rotor_speed / MAX_TRIG_RPM;
+    if(s->feedback.trig_rpm>1.0f)s->feedback.trig_rpm=1.0f;  //如果单环效果好就删
+    if(s->feedback.trig_rpm<-1.0f)s->feedback.trig_rpm=-1.0f;
+    
+    s->errtosee = s->feedback.fric[0].rotor_speed - s->feedback.fric[1].rotor_speed;
+	return 0;
+}
+
+int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd)
+{
+    if (s == NULL || cmd == NULL) {
+        return -1; // 参数错误
+    }
+    s->now = BSP_TIME_Get_us() / 1000000.0f;
+    s->dt = (BSP_TIME_Get_us() - s->lask_wakeup) / 1000000.0f;
+    s->lask_wakeup = BSP_TIME_Get_us();
+	s->online = cmd->online;
+    if(!s->online /*|| s->mode==SHOOT_MODE_SAFE*/){
+        for(int i=0;i<SHOOT_FRIC_NUM;i++)
+        {	
+            MOTOR_RM_Relax(&s->param->fric_motor_param[i]);
+        }
+        MOTOR_RM_Relax(&s->param->trig_motor_param);
     }
     else{
-        switch(c->running_state)
+        switch(s->running_state)
         {
             case SHOOT_STATE_IDLE:/*熄火等待*/
                 for(int i=0;i<SHOOT_FRIC_NUM;i++)
                 {	
-                    PID_ResetIntegral(&c->pid.fric_follow[i]);
+                    PID_ResetIntegral(&s->pid.fric_follow[i]);
-                    c->output.out_follow[i]=PID_Calc(&c->pid.fric_follow[i],0.0f,c->feedback.fric_rpm[i],0,c->dt);
+                    s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i],0.0f,s->feedback.fric_rpm[i],0,s->dt);
-                    c->output.out_fric[i]=c->output.out_follow[i];
+                    s->output.out_fric[i]=s->output.out_follow[i];
-                    c->output.lpfout_fric[i] = LowPassFilter2p_Apply(&c->filter.fric.out[i], c->output.out_fric[i]);
+                    s->output.lpfout_fric[i] = LowPassFilter2p_Apply(&s->filter.fric.out[i], s->output.out_fric[i]);
-                    MOTOR_RM_SetOutput(&c->param->fric_motor_param[i], c->output.lpfout_fric[i]);
+                    MOTOR_RM_SetOutput(&s->param->fric_motor_param[i], s->output.lpfout_fric[i]);
                 }
-                c->output.out_trig=PID_Calc(&c->pid.trig,c->target_variable.target_angle,c->feedback.trig->gearbox_total_angle,0,c->dt);
+                s->output.outagl_trig   =PID_Calc(&s->pid.trig,s->target_variable.target_angle,s->feedback.trig_angle_cicle,0,s->dt);
-                MOTOR_RM_SetOutput(&c->param->trig_motor_param, c->output.out_trig);
+                s->output.outomg_trig  =PID_Calc(&s->pid.trig_omg,s->output.outagl_trig,s->feedback.trig_rpm,0,s->dt);
+                s->output.outlpf_trig   =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig);
+                MOTOR_RM_SetOutput(&s->param->trig_motor_param, s->output.outlpf_trig);
                 if(cmd->ready)
                     {   
-                        c->running_state=SHOOT_STATE_READY;
+                        Shoot_ResetCalu(s);
+                        Shoot_ResetIntegral(s);
+                        Shoot_ResetOutput(s);
+                        s->running_state=SHOOT_STATE_READY;
                     }
                 break;
             case SHOOT_STATE_READY:/*准备射击*/
-                for(int i=0;i<SHOOT_FRIC_NUM;i++)
-                {	/* 计算跟随输出->计算修正输出->加和、滤波、输出 */ 
-                    // c->output.out_follow[i]=PID_Calc(&c->pid.fric_follow[i],c->target_variable.target_rpm/MAX_FRIC_RPM,c->feedback.fric_rpm[i],0,c->dt);
-                    c->output.out_follow[i]=PID_Calc(&c->pid.fric_follow[i],3000.0f/MAX_FRIC_RPM,c->feedback.fric_rpm[i],0,c->dt);
                 
-                    c->output.out_err[i]=PID_Calc(&c->pid.fric_err[i],c->feedback.fric_avgrpm,c->feedback.fric_rpm[i],0,c->dt);
+                for(int i=0;i<SHOOT_FRIC_NUM;i++)
-                    ScaleSumTo1(&c->output.out_follow[i], &c->output.out_err[i]);
+                {	/* 计算跟随输出、计算修正输出 */ 
-                    c->output.out_fric[i]=c->output.out_follow[i]+c->output.out_err[i];
+                    s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i],s->target_variable.target_rpm/MAX_FRIC_RPM,s->feedback.fric_rpm[i],0,s->dt);
-                    c->output.lpfout_fric[i] = LowPassFilter2p_Apply(&c->filter.fric.out[i], c->output.out_fric[i]);
+                    s->output.out_err[i]=PID_Calc(&s->pid.fric_err[i],s->feedback.fric_avgrpm,s->feedback.fric_rpm[i],0,s->dt);
-                    MOTOR_RM_SetOutput(&c->param->fric_motor_param[i], c->output.lpfout_fric[i]);
+                    /* 按比例缩放并加和输出 */
+                    ScaleSumTo1(&s->output.out_follow[i], &s->output.out_err[i]);                    
+                    s->output.out_fric[i]=s->output.out_follow[i]+s->output.out_err[i];
+                    /* 滤波 */
+                    s->output.lpfout_fric[i] = LowPassFilter2p_Apply(&s->filter.fric.out[i], s->output.out_fric[i]);
+                    /* 输出 */
+                    MOTOR_RM_SetOutput(&s->param->fric_motor_param[i], s->output.lpfout_fric[i]);
                 }
-                c->output.out_trig=PID_Calc(&c->pid.trig,c->target_variable.target_angle,c->feedback.trig->gearbox_total_angle,0,c->dt);
+                /* 拨弹电机输出 */
-                MOTOR_RM_SetOutput(&c->param->trig_motor_param, c->output.out_trig);
+                s->output.outagl_trig   =PID_Calc(&s->pid.trig,s->target_variable.target_angle,s->feedback.trig_angle_cicle,0,s->dt);
+                s->output.outomg_trig  =PID_Calc(&s->pid.trig_omg,s->output.outagl_trig,s->feedback.trig_rpm,0,s->dt);
+                s->output.outlpf_trig   =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig);
+                MOTOR_RM_SetOutput(&s->param->trig_motor_param, s->output.outlpf_trig);
+                
+                /* 检查状态机 */
                 if(!cmd->ready)
                     {
-                        c->running_state=SHOOT_STATE_IDLE;
+                        Shoot_ResetCalu(s);
+                        Shoot_ResetOutput(s);
+                        s->running_state=SHOOT_STATE_IDLE;
                     }
-                    else if(cmd->last_firecmd==false&&cmd->firecmd==true)//可以加一个到达目标速度的判断
+                    else if(last_firecmd==false&&cmd->firecmd==true)
                     {
-                        c->running_state=SHOOT_STATE_FIRE;
+                        Shoot_ResetCalu(s);
-						c->target_variable.target_angle-=c->param->trig_step_angle;
+                        Shoot_ResetOutput(s);
+                        s->running_state=SHOOT_STATE_FIRE;
+                        s->shoot_Anglecalu.num_to_shoot+=s->param->shot_burst_num;
+					
                     }
                 break;
             case SHOOT_STATE_FIRE:
-//                c->target_variable.target_angle+=c->param->trig_step_angle;
+				Shoot_CaluTargetAngle(s, cmd);
                 for(int i=0;i<SHOOT_FRIC_NUM;i++)
                 {	
-                    // c->output.out_follow[i]=PID_Calc(&c->pid.fric_follow[i],c->target_variable.target_rpm/MAX_FRIC_RPM,c->feedback.fric_rpm[i],0,c->dt);
+                    s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i],s->target_variable.target_rpm/MAX_FRIC_RPM,s->feedback.fric_rpm[i],0,s->dt);
-                    c->output.out_follow[i]=PID_Calc(&c->pid.fric_follow[i],3000.0f/MAX_FRIC_RPM,c->feedback.fric_rpm[i],0,c->dt);
+                    s->output.out_err[i]=PID_Calc(&s->pid.fric_err[i],s->feedback.fric_avgrpm,s->feedback.fric_rpm[i],0,s->dt);
-                    c->output.out_err[i]=PID_Calc(&c->pid.fric_err[i],c->feedback.fric_avgrpm,c->feedback.fric_rpm[i],0,c->dt);
+                    ScaleSumTo1(&s->output.out_follow[i], &s->output.out_err[i]);
-                    ScaleSumTo1(&c->output.out_follow[i], &c->output.out_err[i]);
+                    s->output.out_fric[i]=s->output.out_follow[i]+s->output.out_err[i];
-                    c->output.out_fric[i]=c->output.out_follow[i]+c->output.out_err[i];
+                    s->output.lpfout_fric[i] = LowPassFilter2p_Apply(&s->filter.fric.out[i], s->output.out_fric[i]);
-                    c->output.lpfout_fric[i] = LowPassFilter2p_Apply(&c->filter.fric.out[i], c->output.out_fric[i]);
+                    MOTOR_RM_SetOutput(&s->param->fric_motor_param[i], s->output.lpfout_fric[i]);
-                    MOTOR_RM_SetOutput(&c->param->fric_motor_param[i], c->output.lpfout_fric[i]);
                 }
-                c->output.out_trig=PID_Calc(&c->pid.trig,c->target_variable.target_angle,c->feedback.trig->gearbox_total_angle,0,c->dt);
+                s->output.outagl_trig   =PID_Calc(&s->pid.trig,s->target_variable.target_angle,s->feedback.trig_angle_cicle,0,s->dt);
-                MOTOR_RM_SetOutput(&c->param->trig_motor_param, c->output.out_trig);
+                s->output.outomg_trig  =PID_Calc(&s->pid.trig_omg,s->output.outagl_trig,s->feedback.trig_rpm,0,s->dt);
+                s->output.outlpf_trig   =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig);
+                MOTOR_RM_SetOutput(&s->param->trig_motor_param, s->output.outlpf_trig);
                 if(!cmd->firecmd)
                     {
-                        c->running_state=SHOOT_STATE_READY;
+                        Shoot_ResetCalu(s);
+                        Shoot_ResetOutput(s);
+                        s->running_state=SHOOT_STATE_READY;
                     }
                 break;
             default:
-                c->running_state=SHOOT_STATE_IDLE;
+                s->running_state=SHOOT_STATE_IDLE;
                 break;
         }
     }
-	MOTOR_RM_Ctrl(&c->param->fric_motor_param[0]);
+	MOTOR_RM_Ctrl(&s->param->fric_motor_param[0]);
-    // BSP_TIME_Delay_us(200);
+    MOTOR_RM_Ctrl(&s->param->trig_motor_param);
-    MOTOR_RM_Ctrl(&c->param->trig_motor_param);
+    last_firecmd = cmd->firecmd;
-    
     return 0;
 }
 

User/module/shoot.h

@@ -23,7 +23,7 @@ extern "C" {
 
 #define SHOOT_FRIC_NUM (2) /* 摩擦轮数量 */
 #define MAX_FRIC_RPM 7000.0f
-#define MAX_TRIG_RPM 1000.0f
+#define MAX_TRIG_RPM 5000.0f
 /* Exported macro ----------------------------------------------------------- */
 /* Exported types ----------------------------------------------------------- */
 
@@ -45,23 +45,28 @@ typedef struct {
   bool ready;                    /* 准备射击 */
   bool firecmd;                  /* 射击指令 */
 
-  bool last_firecmd;
-
 } Shoot_CMD_t;
-
 typedef struct {
   MOTOR_Feedback_t fric[SHOOT_FRIC_NUM]; /* 摩擦轮电机反馈 */
-  MOTOR_RM_t *trig; /* 拨弹电机反馈 */
+  MOTOR_Feedback_t trig; /* 拨弹电机反馈 */
 
   float fil_fric_rpm[SHOOT_FRIC_NUM]; /* 滤波后的摩擦轮转速 */
   float fil_trig_rpm; /* 滤波后的拨弹电机转速*/
 
+  float trig_angle_cicle; /* 拨弹电机减速输出轴单圈角度（0~M_2PI） */
+
   float fric_rpm[SHOOT_FRIC_NUM]; /* 归一化摩擦轮转速 */
   float fric_avgrpm; /* 归一化摩擦轮平均转速*/
   float trig_rpm; /* 归一化拨弹电机转速*/
 
 }Shoot_Feedback_t;
 
+typedef struct{
+  float   time_last_shoot;
+  uint8_t num_to_shoot;
+  uint8_t num_shooted;
+}Shoot_AngleCalu_t;
+
 typedef struct {
   float out_follow[SHOOT_FRIC_NUM];
   float out_err[SHOOT_FRIC_NUM];
@@ -69,14 +74,17 @@ typedef struct {
   float lpfout_fric[SHOOT_FRIC_NUM];
 
   
-  float out_trig;
+  float outagl_trig;
-  float lpfout_trig;
+  float outomg_trig;
+  float outlpf_trig;
 }Shoot_Output_t;
 
 
 /* 底盘参数的结构体，包含所有初始化用的参数，通常是const，存好几组 */
 typedef struct {
-  float trig_step_angle; /* 每次拨弹电机转动的角度 */
+  float trig_step_angle; /* 每发弹丸拨弹电机转动的角度 */
+  float shot_delay_time; /* 射击间隔时间，单位秒 */
+  uint8_t shot_burst_num; /* 多发模式下一次射击的发数 */
 
   MOTOR_RM_Param_t fric_motor_param[SHOOT_FRIC_NUM];
   MOTOR_RM_Param_t trig_motor_param;
@@ -85,7 +93,7 @@ typedef struct {
   KPID_Params_t fric_follow;               /* 摩擦轮电机PID控制参数，用于跟随目标速度 */
   KPID_Params_t fric_err;               /* 摩擦轮电机PID控制参数，用于消除转速误差 */
   KPID_Params_t trig;              /* 拨弹电机PID控制参数 */
-
+  KPID_Params_t trig_omg;              /* 拨弹电机PID控制参数 */
 
   /* 低通滤波器截止频率 */
   struct {
@@ -107,6 +115,7 @@ typedef struct {
 typedef struct {
   bool online;
 
+  float    now;
   uint64_t lask_wakeup;
   float    dt;
 
@@ -117,6 +126,7 @@ typedef struct {
   /* 反馈信息 */
   Shoot_Feedback_t feedback;
   /* 控制信息*/
+  Shoot_AngleCalu_t shoot_Anglecalu;
   Shoot_Output_t output;
   /* 目标控制量 */
   struct {
@@ -129,6 +139,7 @@ typedef struct {
     KPID_t fric_follow[SHOOT_FRIC_NUM]; /* */
     KPID_t fric_err[SHOOT_FRIC_NUM];    /*  */
     KPID_t trig;
+    KPID_t trig_omg;
   } pid;
 
   /* 滤波器 */
@@ -152,32 +163,32 @@ typedef struct {
 /**
  * \brief 初始化发射
  *
- * \param c 包含发射数据的结构体
+ * \param s 包含发射数据的结构体
  * \param param 包含发射参数的结构体指针
  * \param target_freq 任务预期的运行频率
  *
  * \return 函数运行结果
  */
-int8_t Shoot_Init(Shoot_t *c, Shoot_Params_t *param, float target_freq);
+int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq);
 
 /**
  * \brief 更新反馈
  *
- * \param c 包含发射数据的结构体
+ * \param s 包含发射数据的结构体
  *
  * \return 函数运行结果
  */
-int8_t Chassis_UpdateFeedback(Shoot_t *c);
+int8_t Chassis_UpdateFeedback(Shoot_t *s);
 
 /**
  * \brief 初始化发射
  *
- * \param c 包含发射数据的结构体
+ * \param s 包含发射数据的结构体
  * \param cmd 包含发射命令的结构体
  *
  * \return 函数运行结果
  */
-int8_t Shoot_Control(Shoot_t *c, const Shoot_CMD_t *cmd);
+int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd);
 
 
 

debug.jdebug

@@ -22,12 +22,12 @@ void OnProjectLoad (void) {
   //
   // Dialog-generated settings
   //
-  Project.AddPathSubstitute ("/Users/lvzucheng/Documents/R/balance_infantry", "$(ProjectDir)");
-  Project.AddPathSubstitute ("/users/lvzucheng/documents/r/balance_infantry", "$(ProjectDir)");
   Project.SetDevice ("STM32F407IG");
   Project.SetHostIF ("USB", "207400620");
   Project.SetTargetIF ("SWD");
   Project.SetTIFSpeed ("4 MHz");
+  Project.AddPathSubstitute ("/Users/lvzucheng/Documents/R/balance_infantry", "$(ProjectDir)");
+  Project.AddPathSubstitute ("/users/lvzucheng/documents/r/balance_infantry", "$(ProjectDir)");
   Project.AddSvdFile ("$(InstallDir)/Config/CPU/Cortex-M4F.svd");
   //
   // User settings