添加云台数据

MDK-ARM/DevC.uvoptx

@@ -407,6 +407,16 @@
           <WinNumber>1</WinNumber>
           <ItemText>for_shoot</ItemText>
         </Ww>
+        <Ww>
+          <count>10</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>cmd</ItemText>
+        </Ww>
+        <Ww>
+          <count>11</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>host</ItemText>
+        </Ww>
       </WatchWindow1>
       <Tracepoint>
         <THDelay>0</THDelay>
@@ -1738,7 +1748,7 @@
       <GroupNumber>11</GroupNumber>
       <FileNumber>100</FileNumber>
       <FileType>1</FileType>
-      <tvExp>1</tvExp>
+      <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
       <PathWithFileName>..\User\device\ai.c</PathWithFileName>

User/component/cmd.c

@@ -14,12 +14,14 @@
  * @return uint16_t 行为对应的按键
  */
 static inline CMD_KeyValue_t CMD_BehaviorToKey(CMD_t *cmd,
-                                               CMD_Behavior_t behavior) {
+                                               CMD_Behavior_t behavior)
+{
   return cmd->param->map.key_map[behavior].key;
 }
 
 static inline CMD_ActiveType_t CMD_BehaviorToActive(CMD_t *cmd,
-                                                    CMD_Behavior_t behavior) {
+                                                    CMD_Behavior_t behavior)
+{
   return cmd->param->map.key_map[behavior].active;
 }
 
@@ -32,37 +34,47 @@ static inline CMD_ActiveType_t CMD_BehaviorToActive(CMD_t *cmd,
  * @return true 按下
  * @return false 未按下
  */
-static bool CMD_KeyPressedRc(const CMD_RC_t *rc, CMD_KeyValue_t key) {
+static bool CMD_KeyPressedRc(const CMD_RC_t *rc, CMD_KeyValue_t key)
+{
   /* 按下按键为鼠标左、右键 */
-  if (key == CMD_L_CLICK) {
+  if (key == CMD_L_CLICK)
+  {
     return rc->mouse.l_click;
   }
-  if (key == CMD_R_CLICK) {
+  if (key == CMD_R_CLICK)
+  {
     return rc->mouse.r_click;
   }
   return rc->key & (1u << key);
 }
 
 static bool CMD_BehaviorOccurredRc(const CMD_RC_t *rc, CMD_t *cmd,
-                                   CMD_Behavior_t behavior) {
+                                   CMD_Behavior_t behavior)
+{
   CMD_KeyValue_t key = CMD_BehaviorToKey(cmd, behavior);
   CMD_ActiveType_t active = CMD_BehaviorToActive(cmd, behavior);
 
   bool now_key_pressed, last_key_pressed;
 
   /* 按下按键为鼠标左、右键 */
-  if (key == CMD_L_CLICK) {
+  if (key == CMD_L_CLICK)
+  {
     now_key_pressed = rc->mouse.l_click;
     last_key_pressed = cmd->mouse_last.l_click;
-  } else if (key == CMD_R_CLICK) {
+  }
+  else if (key == CMD_R_CLICK)
+  {
     now_key_pressed = rc->mouse.r_click;
     last_key_pressed = cmd->mouse_last.r_click;
-  } else {
+  }
+  else
+  {
     now_key_pressed = rc->key & (1u << key);
     last_key_pressed = cmd->key_last & (1u << key);
   }
 
-  switch (active) {
+  switch (active)
+  {
   case CMD_ACTIVE_PRESSING:
     return now_key_pressed && !last_key_pressed;
   case CMD_ACTIVE_RASING:
@@ -79,7 +91,8 @@ static bool CMD_BehaviorOccurredRc(const CMD_RC_t *rc, CMD_t *cmd,
  * @param cmd 主结构体
  * @param dt_sec 两次解析的间隔
  */
-static void CMD_PcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
+static void CMD_PcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec)
+{
   cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
 
   /* 云台设置为鼠标控制欧拉角的变化，底盘的控制向量设置为零 */
@@ -91,85 +104,111 @@ static void CMD_PcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
   cmd->shoot.reverse_trig = false;
 
   /* 按键行为映射相关逻辑 */
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FORE)) {
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FORE))
+  {
     cmd->chassis.ctrl_vec.vy += cmd->param->move.move_sense;
   }
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BACK)) {
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BACK))
+  {
     cmd->chassis.ctrl_vec.vy -= cmd->param->move.move_sense;
   }
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_LEFT)) {
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_LEFT))
+  {
     cmd->chassis.ctrl_vec.vx -= cmd->param->move.move_sense;
   }
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_RIGHT)) {
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_RIGHT))
+  {
     cmd->chassis.ctrl_vec.vx += cmd->param->move.move_sense;
   }
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_ACCELERATE)) {
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_ACCELERATE))
+  {
     cmd->chassis.ctrl_vec.vx *= cmd->param->move.move_fast_sense;
     cmd->chassis.ctrl_vec.vy *= cmd->param->move.move_fast_sense;
   }
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_DECELEBRATE)) {
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_DECELEBRATE))
+  {
     cmd->chassis.ctrl_vec.vx *= cmd->param->move.move_slow_sense;
     cmd->chassis.ctrl_vec.vy *= cmd->param->move.move_slow_sense;
   }
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FIRE)) {
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FIRE))
+  {
     /* 切换至开火模式，设置相应的射击频率和弹丸初速度 */
     cmd->shoot.mode = SHOOT_MODE_LOADED;
     cmd->shoot.fire = true;
-  } else {
+  }
+  else
+  {
     /* 切换至准备模式，停止射击 */
     cmd->shoot.mode = SHOOT_MODE_LOADED;
     cmd->shoot.fire = false;
   }
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FIRE_MODE)) {
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FIRE_MODE))
+  {
     /* 每按一次依次切换开火下一个模式 */
     cmd->shoot.fire_mode++;
     cmd->shoot.fire_mode %= FIRE_MODE_NUM;
   }
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_ROTOR)) {
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_ROTOR))
+  {
     /* 切换到小陀螺模式 */
     cmd->chassis.mode = CHASSIS_MODE_ROTOR;
     cmd->chassis.mode_rotor = ROTOR_MODE_RAND;
   }
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_OPENCOVER)) {
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_OPENCOVER))
+  {
     /* 每按一次开、关弹舱盖 */
     cmd->shoot.cover_open = !cmd->shoot.cover_open;
   }
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BUFF)) {
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BUFF))
-    if (cmd->ai_status == AI_STATUS_HITSWITCH) {
+  {
+    if (cmd->ai_status == AI_STATUS_HITSWITCH)
+    {
       /* 停止ai的打符模式，停用host控制 */
       CMD_RefereeAdd(&(cmd->referee), CMD_UI_HIT_SWITCH_STOP);
       cmd->host_overwrite = false;
       cmd->ai_status = AI_STATUS_STOP;
-    } else if (cmd->ai_status == AI_STATUS_AUTOAIM) {
+    }
+    else if (cmd->ai_status == AI_STATUS_AUTOAIM)
+    {
       /* 自瞄模式中切换失败提醒 */
-    } else {
+    }
+    else
+    {
       /* ai切换至打符模式，启用host控制 */
       CMD_RefereeAdd(&(cmd->referee), CMD_UI_HIT_SWITCH_START);
       cmd->ai_status = AI_STATUS_HITSWITCH;
       cmd->host_overwrite = true;
     }
   }
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_AUTOAIM)) {
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_AUTOAIM))
-    if (cmd->ai_status == AI_STATUS_AUTOAIM) {
+  {
+    if (cmd->ai_status == AI_STATUS_AUTOAIM)
+    {
       /* 停止ai的自瞄模式，停用host控制 */
       cmd->host_overwrite = false;
       cmd->ai_status = AI_STATUS_STOP;
       CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_STOP);
-    } else {
+    }
+    else
+    {
       /* ai切换至自瞄模式，启用host控制 */
       cmd->ai_status = AI_STATUS_AUTOAIM;
       cmd->host_overwrite = true;
       CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_START);
     }
-  } else {
+  }
+  else
+  {
     cmd->host_overwrite = false;
     // TODO: 修复逻辑
   }
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_REVTRIG)) {
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_REVTRIG))
+  {
     /* 按下拨弹反转 */
     cmd->shoot.reverse_trig = true;
   }
-  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FOLLOWGIMBAL35)) {
+  if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FOLLOWGIMBAL35))
+  {
     cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
   }
   /* 保存当前按下的键位状态 */
@@ -184,27 +223,38 @@ static void CMD_PcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
  * @param cmd 主结构体
  * @param dt_sec 两次解析的间隔
  */
-static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
+static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec)
-  switch (rc->sw_l) {
+{
+  switch (rc->sw_l)
+  {
     /* 左拨杆相应行为选择和解析 */
   case CMD_SW_UP:
     cmd->chassis.mode = CHASSIS_MODE_BREAK;
+    cmd->ai_status = AI_STATUS_STOP;
+    cmd->host_overwrite = false;
     break;
 
   case CMD_SW_MID:
     cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
+    cmd->ai_status = AI_STATUS_AUTOAIM;
+    cmd->host_overwrite = true;
     break;
 
   case CMD_SW_DOWN:
     cmd->chassis.mode = CHASSIS_MODE_ROTOR;
     cmd->chassis.mode_rotor = ROTOR_MODE_CW;
+    cmd->ai_status = AI_STATUS_AUTOAIM;
+    cmd->host_overwrite = true;
     break;
 
   case CMD_SW_ERR:
     cmd->chassis.mode = CHASSIS_MODE_RELAX;
+    cmd->ai_status = AI_STATUS_STOP;
+    cmd->host_overwrite = false;
     break;
   }
-  switch (rc->sw_r) {
+  switch (rc->sw_r)
+  {
     /* 右拨杆相应行为选择和解析*/
   case CMD_SW_UP:
     cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
@@ -222,35 +272,20 @@ static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
     cmd->shoot.mode = SHOOT_MODE_LOADED;
     cmd->shoot.fire_mode = FIRE_MODE_SINGLE;
     cmd->shoot.fire = true;
-      break;
-		/*
-		    case CMD_SW_UP:
-      cmd->gimbal.mode = GIMBAL_MODE_RELAX;
-      cmd->shoot.mode = SHOOT_MODE_SAFE;
     break;
 
-    case CMD_SW_MID:
-      cmd->gimbal.mode = GIMBAL_MODE_RELAX;
-      cmd->shoot.fire = false;
-      cmd->shoot.mode = SHOOT_MODE_LOADED;
-      break;
-
-    case CMD_SW_DOWN:
-      cmd->gimbal.mode = GIMBAL_MODE_RELAX;
-      cmd->shoot.mode = SHOOT_MODE_LOADED;
-      cmd->shoot.fire_mode = FIRE_MODE_SINGLE;
-      cmd->shoot.fire = true;
-      break;
-		*/
   case CMD_SW_ERR:
     cmd->gimbal.mode = GIMBAL_MODE_RELAX;
     cmd->shoot.mode = SHOOT_MODE_RELAX;
   }
   /* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */
+  if (cmd->ai_status != AI_STATUS_STOP || cmd->host_overwrite == false)
+  {
     cmd->chassis.ctrl_vec.vx = rc->ch_l_x;
     cmd->chassis.ctrl_vec.vy = rc->ch_l_y;
     cmd->gimbal.delta_eulr.yaw = rc->ch_r_x * dt_sec * cmd->param->sens_rc;
     cmd->gimbal.delta_eulr.pit = rc->ch_r_y * dt_sec * cmd->param->sens_rc;
+  }
 }
 
 /**
@@ -258,7 +293,8 @@ static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
  *
  * @param cmd 主结构体
  */
-static void CMD_RcLostLogic(CMD_t *cmd) {
+static void CMD_RcLostLogic(CMD_t *cmd)
+{
   /* 机器人底盘、云台、射击运行模式恢复至放松模式 */
   cmd->chassis.mode = CHASSIS_MODE_RELAX;
   cmd->gimbal.mode = GIMBAL_MODE_RELAX;
@@ -272,10 +308,13 @@ static void CMD_RcLostLogic(CMD_t *cmd) {
  * @param param 参数
  * @return int8_t 0对应没有错误
  */
-int8_t CMD_Init(CMD_t *cmd, const CMD_Params_t *param) {
+int8_t CMD_Init(CMD_t *cmd, const CMD_Params_t *param)
+{
   /* 指针检测 */
-  if (cmd == NULL) return -1;
+  if (cmd == NULL)
-  if (param == NULL) return -1;
+    return -1;
+  if (param == NULL)
+    return -1;
 
   /* 设置机器人的命令参数，初始化控制方式为rc控制 */
   cmd->pc_ctrl = false;
@@ -301,10 +340,13 @@ inline bool CMD_CheckHostOverwrite(CMD_t *cmd) { return cmd->host_overwrite; }
  * @param dt_sec 两次解析的间隔
  * @return int8_t 0对应没有错误
  */
-int8_t CMD_ParseRc(CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
+int8_t CMD_ParseRc(CMD_RC_t *rc, CMD_t *cmd, float dt_sec)
+{
   /* 指针检测 */
-  if (rc == NULL) return -1;
+  if (rc == NULL)
-  if (cmd == NULL) return -1;
+    return -1;
+  if (cmd == NULL)
+    return -1;
 
   /* 在pc控制和rc控制间切换 */
   if (CMD_KeyPressedRc(rc, CMD_KEY_SHIFT) &&
@@ -315,12 +357,18 @@ int8_t CMD_ParseRc(CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
       CMD_KeyPressedRc(rc, CMD_KEY_CTRL) && CMD_KeyPressedRc(rc, CMD_KEY_E))
     cmd->pc_ctrl = false;
   /*c当rc丢控时，恢复机器人至默认状态 */
-  if ((rc->sw_l == CMD_SW_ERR) || (rc->sw_r == CMD_SW_ERR)) {
+  if ((rc->sw_l == CMD_SW_ERR) || (rc->sw_r == CMD_SW_ERR))
+  {
     CMD_RcLostLogic(cmd);
-  } else {
+  }
-    if (cmd->pc_ctrl) {
+  else
+  {
+    if (cmd->pc_ctrl)
+    {
       CMD_PcLogic(rc, cmd, dt_sec);
-    } else {
+    }
+    else
+    {
       CMD_RcLogic(rc, cmd, dt_sec);
     }
   }
@@ -335,21 +383,27 @@ int8_t CMD_ParseRc(CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
  * @param dt_sec 两次解析的间隔
  * @return int8_t 0对应没有错误
  */
-int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec) {
+int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec)
+{
   (void)dt_sec; /* 未使用dt_sec，消除警告 */
   /* 指针检测 */
-  if (host == NULL) return -1;
+  if (host == NULL)
-  if (cmd == NULL) return -1;
+    return -1;
+  if (cmd == NULL)
+    return -1;
 
   /* 云台欧拉角设置为host相应的变化的欧拉角 */
   cmd->gimbal.delta_eulr.yaw = host->gimbal_delta.yaw;
   cmd->gimbal.delta_eulr.pit = host->gimbal_delta.pit;
 
   /* host射击命令，设置不同的射击频率和弹丸初速度 */
-  if (host->fire) {
+  if (host->fire)
+  {
     cmd->shoot.mode = SHOOT_MODE_LOADED;
     cmd->shoot.fire = true;
-  } else {
+  }
+  else
+  {
     cmd->shoot.mode = SHOOT_MODE_SAFE;
   }
   return 0;
@@ -362,11 +416,14 @@ int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec) {
  * @param cmd 要添加的命令
  * @return int8_t 0对应没有错误
  */
-int8_t CMD_RefereeAdd(CMD_RefereeCmd_t *ref, CMD_UI_t cmd) {
+int8_t CMD_RefereeAdd(CMD_RefereeCmd_t *ref, CMD_UI_t cmd)
+{
   /* 指针检测 */
-  if (ref == NULL) return -1;
+  if (ref == NULL)
+    return -1;
   /* 越界检测 */
-  if (ref->counter >= CMD_REFEREE_MAX_NUM || ref->counter < 0) return -1;
+  if (ref->counter >= CMD_REFEREE_MAX_NUM || ref->counter < 0)
+    return -1;
 
   /* 添加机器人当前行为状态到画图的命令队列中 */
   ref->cmd[ref->counter] = cmd;

User/device/ai.c

@@ -88,9 +88,10 @@ error:
 }
 
 void AI_PackCmd(AI_t *ai, CMD_Host_t *cmd_host) {
-  cmd_host->gimbal_delta.yaw = -ai->form_host.data.gimbal.yaw;
+  cmd_host->gimbal_delta.yaw = ai->form_host.data.gimbal.yaw;
-  cmd_host->gimbal_delta.pit = -ai->form_host.data.gimbal.pit;
+  cmd_host->gimbal_delta.pit = ai->form_host.data.gimbal.pit;
-  cmd_host->fire = (ai->form_host.data.notice & AI_NOTICE_FIRE);
+  // cmd_host->fire = (ai->form_host.data.notice & AI_NOTICE_FIRE);
+  cmd_host->fire = true;
   cmd_host->chassis_move_vec.vx = ai->form_host.data.chassis_move_vec.vx;
   cmd_host->chassis_move_vec.vy = ai->form_host.data.chassis_move_vec.vy;
   cmd_host->chassis_move_vec.wz = ai->form_host.data.chassis_move_vec.wz;
@@ -105,10 +106,12 @@ int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *cmd_host) {
   return 0;
 }
 
-int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat) {
+int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat, const AHRS_Eulr_t *gimbal_ai) {
   ai->to_host.mcu.id = AI_ID_MCU;
   memcpy((void *)&(ai->to_host.mcu.package.data.quat), (const void *)quat,
          sizeof(*quat));
+  memcpy((void *)&(ai->to_host.mcu.package.data.gimbal), (const void *)gimbal_ai,
+         sizeof(*gimbal_ai));
   ai->to_host.mcu.package.data.notice = 0;
   if (ai->status == AI_STATUS_AUTOAIM)
     ai->to_host.mcu.package.data.notice |= AI_NOTICE_AUTOAIM;

User/device/ai.h

@@ -57,7 +57,7 @@ int8_t AI_StartReceiving(AI_t *ai);
 bool AI_WaitDmaCplt(void);
 int8_t AI_ParseHost(AI_t *ai);
 int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *cmd_host);
-int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat);
+int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat, const AHRS_Eulr_t *gimbal_ai);
 int8_t AI_PackRef(AI_t *ai, const Referee_ForAI_t *ref);
 int8_t AI_StartSend(AI_t *ai, bool option);
 void AI_PackCmd(AI_t *ai, CMD_Host_t *cmd_host);

User/device/can.h

@@ -72,7 +72,7 @@ typedef enum {
 
   CAN_M3508_FRIC1_ID = 0x205, /* 5 */
   CAN_M3508_FRIC2_ID = 0x206, /* 6 */
-  CAN_M2006_TRIG_ID = 0x208,  /* 8 */
+  CAN_M2006_TRIG_ID = 0x207,  /* 8 */
 
   CAN_GM6020_YAW_ID = 0x209, /* 5 */
   CAN_GM6020_PIT_ID = 0x20A, /* 6 */

User/module/config.c

@@ -511,14 +511,14 @@ static const Config_RobotParam_t param_sentry_left = {
         .trig = -1.0f,
       },
     },
-    .num_trig_tooth = 10.0f,
+    .num_trig_tooth = 8.0f,
     .trig_gear_ratio = 36.0f,
     .fric_radius = 0.03f,
     .cover_open_duty = 0.10f,
     .cover_close_duty = 0.050f,
     .model = SHOOT_MODEL_17MM,
-    .bullet_speed = 30.f,
+    .bullet_speed = 25.f,
-    .min_shoot_delay = (uint32_t)(1000.0f / 10.0f),
+    .min_shoot_delay = (uint32_t)(1000.0f / 3.0f),
   }, /* shoot */
 
   .can = {

User/module/gimbal.c

@@ -201,6 +201,20 @@ int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now) {
   return 0;
 }
 
+/**
+ * \brief 打包云台数据给AI
+ *
+ * \param g 包含云台数据的结构体
+ * \param gimbal_for_ai 云台AI数据
+ *
+ * \return 函数运行结果
+ */
+uint8_t Gimbal_PackAI(Gimbal_t *g, const AHRS_Eulr_t *ai){
+  memcpy((void *)ai, (const void *)&(g->feedback.eulr.encoder),
+  sizeof(g->feedback.eulr.encoder));
+return 0;
+}
+
 /**
  * \brief 复制云台输出值
  *

User/module/gimbal.h

@@ -145,6 +145,16 @@ int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal, const CAN_t *can);
  */
 int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now);
 
+/**
+ * \brief 打包云台数据给AI
+ *
+ * \param g 包含云台数据的结构体
+ * \param gimbal_for_ai 云台AI数据
+ *
+ * \return 函数运行结果
+ */
+uint8_t Gimbal_PackAI(Gimbal_t *g, const AHRS_Eulr_t *ai);
+
 /**
  * \brief 复制云台输出值
  *

User/module/shoot.c

@@ -248,6 +248,7 @@ int8_t Shoot_Control(Shoot_t *s, CMD_ShootCmd_t *s_cmd,
   s->setpoint.fric_rpm[1] =
       CalculateRpm(s->fire_ctrl.bullet_speed, s->param->fric_radius,
                    (s->param->model == SHOOT_MODEL_17MM));
+  s->setpoint.fric_rpm[1] = -s->setpoint.fric_rpm[1];
   s->setpoint.fric_rpm[0] = -s->setpoint.fric_rpm[1];
 
   /* 计算拨弹电机位置的目标值 */
@@ -255,10 +256,10 @@ int8_t Shoot_Control(Shoot_t *s, CMD_ShootCmd_t *s_cmd,
       (s_cmd->fire)) {
     /* 将拨弹电机角度进行循环加法，每次加(减)射出一颗弹丸的弧度变化 */
     if (s_cmd->reverse_trig) { /* 反转拨弹 */
-      CircleAdd(&(s->setpoint.trig_angle), M_2PI / s->param->num_trig_tooth,
+      CircleAdd(&(s->setpoint.trig_angle), -M_2PI / s->param->num_trig_tooth,
                 M_2PI);
     } else {
-      CircleAdd(&(s->setpoint.trig_angle), -M_2PI / s->param->num_trig_tooth,
+      CircleAdd(&(s->setpoint.trig_angle), M_2PI / s->param->num_trig_tooth,
                 M_2PI);
       s->fire_ctrl.shooted++;
       s->fire_ctrl.last_shoot = now;

User/task/ai.c

@@ -17,11 +17,13 @@ AI_t ai;
 CMD_Host_t cmd_host;
 AHRS_Quaternion_t quat;
 Referee_ForAI_t referee_ai;
+AHRS_Eulr_t gimbal_ai;
 #else
 static AI_t ai;
 static CMD_Host_t cmd_host;
 static AHRS_Quaternion_t quat;
 static Referee_ForAI_t referee_ai;
+static AHRS_Eulr_t gimbal_ai;
 #endif
 
 /* Private function --------------------------------------------------------- */
@@ -66,10 +68,11 @@ void Task_Ai(void *argument) {
 
     osMessageQueueGet(task_runtime.msgq.ai.quat, &(quat), NULL, 0);
     osMessageQueueGet(task_runtime.msgq.cmd.ai, &(ai.status), NULL, 0);
+    osMessageQueueGet(task_runtime.msgq.gimbal.eulr_encoder, &(gimbal_ai), NULL, 0);
     bool ref_update = (osMessageQueueGet(task_runtime.msgq.referee.ai,
                                          &(referee_ai), NULL, 0) == osOK);
     
-    AI_PackMCU(&ai, &quat);
+    AI_PackMCU(&ai, &quat, &gimbal_ai);
     if (ref_update) AI_PackRef(&ai, &(referee_ai));
 
     AI_StartSend(&(ai), ref_update);

User/task/command.c

@@ -68,7 +68,6 @@ void Task_Command(void *argument) {
       if (osMessageQueueGet(task_runtime.msgq.cmd.raw.host, &host, 0, 0) ==
           osOK)
         CMD_ParseHost(&host, &cmd, 1.0f / (float)TASK_FREQ_CTRL_COMMAND);
-
     osKernelUnlock(); /* 锁住RTOS内核防止控制过程中断，造成错误 */
 
     /* 将需要与其他任务分享的数据放到消息队列中 */

User/task/ctrl_gimbal.c

@@ -22,11 +22,13 @@ CMD_GimbalCmd_t gimbal_cmd;
 Gimbal_t gimbal;
 CAN_GimbalOutput_t gimbal_out;
 Referee_GimbalUI_t gimbal_ui;
+AHRS_Eulr_t gimbal_for_ai;
 #else
 static CMD_GimbalCmd_t gimbal_cmd;
 static Gimbal_t gimbal;
 static CAN_GimbalOutput_t gimbal_out;
 static Referee_GimbalUI_t gimbal_ui;
+static AHRS_Eulr_t gimbal_for_ai;
 #endif
 
 /* Private function --------------------------------------------------------- */
@@ -66,12 +68,15 @@ void Task_CtrlGimbal(void *argument) {
 
     osKernelLock(); /* 锁住RTOS内核防止控制过程中断，造成错误 */
     Gimbal_UpdateFeedback(&gimbal, &can);
+    Gimbal_PackAI(&gimbal, &gimbal_for_ai);
     Gimbal_Control(&gimbal, &gimbal_cmd, tick);
     Gimbal_DumpOutput(&gimbal, &gimbal_out);
 
     osKernelUnlock();
     osMessageQueueReset(task_runtime.msgq.can.output.gimbal);
     osMessageQueuePut(task_runtime.msgq.can.output.gimbal, &gimbal_out, 0, 0);
+    osMessageQueueReset(task_runtime.msgq.gimbal.eulr_encoder);
+    osMessageQueuePut(task_runtime.msgq.gimbal.eulr_encoder, &gimbal_for_ai, 0, 0);
 
     Gimbal_DumpUI(&gimbal, &gimbal_ui);
     osMessageQueueReset(task_runtime.msgq.ui.gimbal);

User/task/init.c

@@ -30,7 +30,8 @@
  *
  * \param argument 未使用
  */
-void Task_Init(void *argument) {
+void Task_Init(void *argument)
+{
     (void)argument; /* 未使用argument，消除警告 */
 
     Config_Get(&task_runtime.cfg); /* 获取机器人配置 */
@@ -96,7 +97,8 @@ void Task_Init(void *argument) {
         osMessageQueueNew(2u, sizeof(AHRS_Eulr_t), NULL);
     task_runtime.msgq.gimbal.gyro =
         osMessageQueueNew(2u, sizeof(AHRS_Gyro_t), NULL);
-
+    task_runtime.msgq.gimbal.eulr_encoder =
+        osMessageQueueNew(2u, sizeof(AHRS_Eulr_t), NULL);
     task_runtime.msgq.cap_info =
         osMessageQueueNew(2u, sizeof(CAN_Capacitor_t), NULL);
 

User/task/user_task.h

@@ -58,6 +58,7 @@ typedef struct {
       osMessageQueueId_t accl;     /* IMU读取 */
       osMessageQueueId_t gyro;     /* IMU读取 */
       osMessageQueueId_t eulr_imu; /* 姿态解算得到 */
+      osMessageQueueId_t eulr_encoder; /* 编码器读取 */
     } gimbal;
 
     /* 控制指令 */