From 95c0c4823c6cf8c8936f308d4faf6149b1797070 Mon Sep 17 00:00:00 2001
From: Marine_zxy <1423266892@qq.com>
Date: Sun, 21 Apr 2024 12:31:55 +0800
Subject: [PATCH 01/19] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E5=BA=95=E7=9B=98?=
 =?UTF-8?q?=E5=85=A8=E5=90=91=E5=8D=81=E5=AD=97=E8=A7=A3=E7=AE=97?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/component/comp_mixer.cpp | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/src/component/comp_mixer.cpp b/src/component/comp_mixer.cpp
index 301ed737..ead64bac 100644
--- a/src/component/comp_mixer.cpp
+++ b/src/component/comp_mixer.cpp
@@ -78,11 +78,14 @@ bool Mixer::Apply(Component::Type::MoveVector &move_vec, float *out) {
       out[3] = -move_vec.vx - move_vec.vy + move_vec.wz;
       break;
     case OMNIPLUS:
+      XB_ASSERT(this->len_ == 4);
+      out[0] = move_vec.vx + move_vec.wz;
+      out[1] = move_vec.vy + move_vec.wz;
+      out[2] = -move_vec.vx + move_vec.wz;
+      out[3] = -move_vec.vy + move_vec.wz;
       break;
 
     case NONE:
-      break;
-
     default:
       break;
   }
-- 
Gitee


From 1f58f94740f513fbdabbdded37990b2c5b1c9977 Mon Sep 17 00:00:00 2001
From: Marine_zxy <1423266892@qq.com>
Date: Sun, 21 Apr 2024 16:00:09 +0800
Subject: [PATCH 02/19] =?UTF-8?q?=E5=AE=8C=E5=96=84=E8=A3=81=E5=88=A4?=
 =?UTF-8?q?=E7=B3=BB=E7=BB=9F?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/device/referee/dev_referee.hpp | 21 ++++++++++++++-------
 1 file changed, 14 insertions(+), 7 deletions(-)

diff --git a/src/device/referee/dev_referee.hpp b/src/device/referee/dev_referee.hpp
index a7a76e12..f4ee93cd 100644
--- a/src/device/referee/dev_referee.hpp
+++ b/src/device/referee/dev_referee.hpp
@@ -76,11 +76,11 @@ class Referee {
     REF_CMD_ID_RADAR_MARK = 0X020C,
     REF_CMD_ID_SENTRY_DECISION = 0x020D, /* 哨兵自主决策相关信息同步 */
     REF_CMD_ID_RADAR_DECISION = 0x020E, /* 雷达自主决策相关信息同步 */
-    REF_CMD_ID_INTER_STUDENT = 0x0301,
-    REF_CMD_ID_INTER_STUDENT_CUSTOM = 0x0302,
+    REF_CMD_ID_INTER_STUDENT = 0x0301,        /* 机器人交互数据 */
+    REF_CMD_ID_INTER_STUDENT_CUSTOM = 0x0302, /* 自定义控制器和机器人 */
     REF_CMD_ID_CLIENT_MAP = 0x0303,
     REF_CMD_ID_KEYBOARD_MOUSE = 0x0304,
-    REF_CMD_ID_MAP_ROBOT_DATA = 0x0305,  // 0x0305
+    REF_CMD_ID_MAP_ROBOT_DATA = 0x0305,
     REF_CMD_ID_CUSTOM_KEYBOARD_MOUSE = 0X0306,
     REF_CMD_ID_SENTRY_POS_DATA = 0x0307,
     REF_CMD_ID_ROBOT_POS_DATA = 0x0308, /* 选手端小地图接受机器人消息 */
@@ -288,7 +288,7 @@ class Referee {
     uint8_t res : 5;
   } RadarInfo; /* 0x020E */
   typedef struct __attribute__((packed)) {
-    uint16_t data_cmd_id;
+    uint16_t data_cmd_id; /* 子内容ID */
     uint16_t sender_id;
     uint16_t receiver_id;
     std::array<int8_t, 113> user_data;
@@ -355,7 +355,10 @@ class Referee {
     REF_BOT_RED_INFANTRY_3 = 5,
     REF_BOT_RED_DRONE = 6,
     REF_BOT_RED_SENTRY = 7,
+    REF_BOT_RED_DART = 8,
     REF_BOT_RED_RADER = 9,
+    REF_BOT_RED_OUTPOST = 10,
+    REF_BOT_RED_BASE = 11,
     REF_BOT_BLU_HERO = 101,
     REF_BOT_BLU_ENGINEER = 102,
     REF_BOT_BLU_INFANTRY_1 = 103,
@@ -363,7 +366,10 @@ class Referee {
     REF_BOT_BLU_INFANTRY_3 = 105,
     REF_BOT_BLU_DRONE = 106,
     REF_BOT_BLU_SENTRY = 107,
+    REF_BOT_BLU_DART = 108,
     REF_BOT_BLU_RADER = 109,
+    REF_BOT_BLU_OUTPOST = 110,
+    REF_BOT_BLU_BASE = 111,
   } RobotID;
 
   typedef enum {
@@ -379,7 +385,8 @@ class Referee {
     REF_CL_BLU_INFANTRY_2 = 0x0168,
     REF_CL_BLU_INFANTRY_3 = 0x0169,
     REF_CL_BLU_DRONE = 0x016A,
-  } ClientID;
+    REF_CL_REFEREE_SERVER = 0x8080, /* 用于哨兵和雷达自主决策 */
+  } ClientID;                       /* 子内容ID */
 
   typedef enum {
     REF_STDNT_CMD_ID_UI_DEL = 0x0100,
@@ -389,8 +396,8 @@ class Referee {
     REF_STDNT_CMD_ID_UI_DRAW7 = 0x0104,
     REF_STDNT_CMD_ID_UI_STR = 0x0110,
     REF_STDNT_CMD_ID_CUSTOM = 0x0200,
-    REF_STDNT_CMD_ID_SENTRY_COMMAND = 0X0120,
-    REF_STDNT_CMD_ID_RADAR_COMMAND = 0X0121,
+    REF_STDNT_CMD_ID_SENTRY_CMD = 0X0120,
+    REF_STDNT_CMD_ID_RADAR_CMD = 0X0121,
   } CMDID;
 
   typedef struct __attribute__((packed)) {
-- 
Gitee


From 660bcb582199eb3b6a5097e3e5e6fe218b3e4de3 Mon Sep 17 00:00:00 2001
From: Marine_zxy <1423266892@qq.com>
Date: Sat, 4 May 2024 11:17:24 +0800
Subject: [PATCH 03/19] =?UTF-8?q?=E8=A3=81=E5=88=A4=E7=B3=BB=E7=BB=9F1.6.2?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/device/referee/dev_referee.hpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/device/referee/dev_referee.hpp b/src/device/referee/dev_referee.hpp
index f4ee93cd..872ed664 100644
--- a/src/device/referee/dev_referee.hpp
+++ b/src/device/referee/dev_referee.hpp
@@ -291,8 +291,8 @@ class Referee {
     uint16_t data_cmd_id; /* 子内容ID */
     uint16_t sender_id;
     uint16_t receiver_id;
-    std::array<int8_t, 113> user_data;
-    /*最大值113*/
+    std::array<int8_t, 112> user_data;
+    /*最大值112*/
   } RobotInteractionData; /* 0x0301 */
 
   typedef struct __attribute__((packed)) {
-- 
Gitee


From ecd7dc9428e34839455ce5556a5499684424afaf Mon Sep 17 00:00:00 2001
From: Marine_zxy <1423266892@qq.com>
Date: Sat, 4 May 2024 23:01:22 +0800
Subject: [PATCH 04/19] =?UTF-8?q?=E5=93=A8=E5=85=B5=E5=88=9D=E6=AD=A5?=
 =?UTF-8?q?=E5=AE=8C=E5=96=84?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/component/comp_cmd.hpp         |  2 +-
 src/device/ai/dev_ai.cpp           | 42 ++++++++++++++++++++++++++++--
 src/device/ai/dev_ai.hpp           | 29 +++++++++++++++++++++
 src/device/referee/dev_referee.cpp | 27 +++++++++++++++++--
 src/device/referee/dev_referee.hpp | 25 ++++++++++++++++--
 src/module/gimbal/mod_gimbal.cpp   | 16 ++++++++++++
 src/module/gimbal/mod_gimbal.hpp   | 12 +++++++++
 src/robot/sentry/robot.cpp         |  4 +++
 8 files changed, 150 insertions(+), 7 deletions(-)

diff --git a/src/component/comp_cmd.hpp b/src/component/comp_cmd.hpp
index b96c9c6d..5b4111ee 100644
--- a/src/component/comp_cmd.hpp
+++ b/src/component/comp_cmd.hpp
@@ -9,7 +9,7 @@ class CMD {
   typedef enum { GIMBAL_ABSOLUTE_CTRL, GIMBAL_RELATIVE_CTRL } GimbalCommandMode;
 
   typedef enum {
-    CTRL_SOURCE_RC,
+    CTRL_SOURCE_RC, /* 键鼠控制源 */
     CTRL_SOURCE_AI,
     CTRL_SOURCE_TERM,
     CTRL_SOURCE_EXT,
diff --git a/src/device/ai/dev_ai.cpp b/src/device/ai/dev_ai.cpp
index 4e2fb2a8..1e73c025 100644
--- a/src/device/ai/dev_ai.cpp
+++ b/src/device/ai/dev_ai.cpp
@@ -46,6 +46,7 @@ AI::AI()
       }
 
       /* 发布控制命令 */
+      ai->DecideAction();
       ai->PackCMD();
 
       /* 发送数据到上位机 */
@@ -134,8 +135,45 @@ bool AI::PackRef() {
 
   return true;
 }
+void AI::DecideAction() {
+  /* 后续通信协议要改：
+  1、导航到哪里
+  2、……
+  */
+
+  /* 确认复活要写！*/
+  /* 根据AI数据、事件、受击、血量来决定底盘、云台、发射机构行为模式 */
+
+  /* 底盘 */
+  /* 无导航需求：前哨、基地、机器人血量 */
+  if (0 /* 导航*/) { /* 导航 */
+    this->action_.ai_chassis = AI::Action::TO_BASE;
+  } else { /* 小陀螺 */
+    this->action_.ai_chassis = AI::Action::ROTOR;
+  }
+
+  /* 云台 */
+  if (this->notice_ == AI_NOTICE_AUTO_AIM) { /* 自瞄 */
+    this->action_.ai_gimbal = AI::Action::AUTO_AIM;
+    this->cmd_.gimbal.mode = Component::CMD::GIMBAL_ABSOLUTE_CTRL;
+  } else if (0) { /* 反击 */
+    this->action_.ai_gimbal = AI::Action::AFFECTED;
+  } else { /* 扫描 */
+    this->action_.ai_gimbal = AI::Action::SCANF;
+  }
+  /* 发射 */
+  if (this->notice_ == AI_NOTICE_FIRE) { /* 开火 */
+    this->action_.ai_launcher = AI::Action::FIRE;
+  } else {
+    this->action_.ai_launcher = AI::Action::CEASEFIRE; /* 不发弹 */
+  }
+  /* 裁判系统：确认复活、购买发弹量、购买血量 */
+}
 
 bool AI::PackCMD() {
+  /* 根据模式，来发送不同的数据 */
+  /* 数据：*/
+  /* 发布控制命令 -> cmd结构体 */
   memcpy(&(this->cmd_.gimbal.eulr), &(this->from_host_.data.gimbal),
          sizeof(this->cmd_.gimbal.eulr));
 
@@ -147,7 +185,7 @@ bool AI::PackCMD() {
          sizeof(this->from_host_.data.chassis_move_vec));
 
   this->notice_ = this->from_host_.data.notice;
-
+  /* 以上是ai的数据直接发布 */
   memcpy(&(this->last_eulr_), &(this->cmd_.gimbal.eulr),
          sizeof(this->cmd_.gimbal.eulr));
 
@@ -156,7 +194,7 @@ bool AI::PackCMD() {
   this->cmd_tp_.Publish(this->cmd_);
 
   if (!Component::CMD::Online()) {
-    return false;
+    return false; /* 如果遥控器不在线 */
   }
 
   /* 控制权在AI */
diff --git a/src/device/ai/dev_ai.hpp b/src/device/ai/dev_ai.hpp
index bdbc56a6..f9f9fbba 100644
--- a/src/device/ai/dev_ai.hpp
+++ b/src/device/ai/dev_ai.hpp
@@ -38,7 +38,32 @@ class AI {
     AI_TURN,
     AI_FIRE_COMMAND,
   } AIControlData;
+  typedef enum {
+    TO_BASE,
+    TO_OUTPOST,
+    TO_SUPPLY,
+    TO_HIGHLAND,
+    TO_PATROL_AREA, /* 基地前巡逻区 */
+    ROTOR,
+
+    SCANF,
+    AUTO_AIM,
+    AFFECTED, /* 反击*/
+
+    CEASEFIRE, /* 停火 */
+    FIRE,
+
+    CONFIRM_RESURRECTION, /* 确认复活 */
+    EXCHANGE_BULLETS,     /* 兑换弹丸 */
+    EXCHANGE_HP,          /* 兑换血量 */
+  } Action;
 
+  typedef struct {
+    uint8_t ai_chassis;    /* 小陀螺、导航 */
+    uint8_t ai_gimbal;     /* 扫描、自瞄、反击 */
+    uint8_t ai_launcher;   /* 不发弹丸、开火 */
+    uint8_t ai_to_referee; /* 确认复活、购买发弹量、购买血量 */
+  } AICtrlAction;
   AI();
 
   bool StartRecv();
@@ -57,6 +82,8 @@ class AI {
 
   bool PackCMD();
 
+  void DecideAction();
+
  private:
   bool ref_updated_ = false;
 
@@ -83,6 +110,8 @@ class AI {
 
   Component::CMD::Data cmd_{};
 
+  AICtrlAction action_;
+
   struct {
     float yaw; /* 偏航角（Yaw angle） */
     float pit; /* 俯仰角（Pitch angle） */
diff --git a/src/device/referee/dev_referee.cpp b/src/device/referee/dev_referee.cpp
index 5f200910..33949583 100644
--- a/src/device/referee/dev_referee.cpp
+++ b/src/device/referee/dev_referee.cpp
@@ -101,7 +101,7 @@ Referee::Referee() : event_(Message::Event::FindEvent("cmd_event")) {
     uint32_t last_online_time = bsp_time_get_ms();
 
     while (1) {
-      ref->UpdateUI();
+      ref->UpdateUI(); /* 更新UI，即机器人to裁判系统 */
       ref->trans_thread_.SleepUntil(40, last_online_time);
     }
   };
@@ -318,6 +318,7 @@ bool Referee::UpdateUI() {
   uint32_t ele_counter = 0;
   uint32_t pack_size = 0;
   CMDID cmd_id = REF_STDNT_CMD_ID_UI_DEL;
+
   Component::UI::GraphicOperation op = Component::UI::UI_GRAPHIC_OP_NOTHING;
 
   if (!done && this->static_del_data_.Size() > 0) {
@@ -377,7 +378,6 @@ bool Referee::UpdateUI() {
 
     static_string_data_.Reset();
   }
-
   if (!done && this->del_data_.Size() > 0) {
     cmd_id = REF_STDNT_CMD_ID_UI_DEL;
     pack_size = sizeof(UIDelPack);
@@ -487,6 +487,29 @@ bool Referee::UpdateUI() {
   return true;
 }
 
+bool Referee::SentryDecision() {
+  uint32_t pack_size = 0;
+  pack_size = sizeof(SentryPack);
+  this->packet_sent_.Wait(UINT32_MAX);  //上锁
+
+  this->sentry_lock_.Wait(UINT32_MAX);  //上锁
+
+  this->sentry_pack_.cmd_id = REF_CMD_ID_INTER_STUDENT;  // 0x0301
+
+  this->sentry_pack_.sentry_cmd = 1; /* 待完善 */
+
+  SetUIHeader(this->sentry_pack_.student_header, REF_STDNT_CMD_ID_SENTRY_CMD,
+              static_cast<RobotID>(this->ref_data_.robot_status.robot_id));
+  //设置子ID、发送者、接受者
+
+  SetPacketHeader(this->sentry_pack_.frame_header, pack_size - 9);
+  //设置通信协议
+  bsp_uart_transmit(BSP_UART_REF,
+                    reinterpret_cast<uint8_t *>(&this->sentry_pack_), pack_size,
+                    false);
+  return true;
+}
+
 bool Referee::AddUI(Component::UI::Ele ui_data) {
   self_->ui_lock_.Wait(UINT32_MAX);
   if (ui_data.op == Component::UI::UI_GRAPHIC_OP_ADD) {
diff --git a/src/device/referee/dev_referee.hpp b/src/device/referee/dev_referee.hpp
index 872ed664..6780b859 100644
--- a/src/device/referee/dev_referee.hpp
+++ b/src/device/referee/dev_referee.hpp
@@ -385,7 +385,7 @@ class Referee {
     REF_CL_BLU_INFANTRY_2 = 0x0168,
     REF_CL_BLU_INFANTRY_3 = 0x0169,
     REF_CL_BLU_DRONE = 0x016A,
-    REF_CL_REFEREE_SERVER = 0x8080, /* 用于哨兵和雷达自主决策 */
+    REF_CL_REFEREE_SERVER = 0x8080, /* 裁判系统服务器，用于哨兵和雷达自主决策 */
   } ClientID;                       /* 子内容ID */
 
   typedef enum {
@@ -494,10 +494,26 @@ class Referee {
     struct __attribute__((packed)) {
       Header frame_header;
       uint16_t cmd_id;
-      Referee::InterStudentHeader student_header;
+      Referee::InterStudentHeader student_header;  //字命令、发送者、接受者
     } raw;
   };
 
+  typedef struct __attribute__((packed)) {
+    Header frame_header;  // 0x0301
+    uint16_t cmd_id;
+    Referee::InterStudentHeader student_header;  //含0x0102
+    uint32_t sentry_cmd;
+    uint16_t crc16;
+  } SentryPack;
+
+  typedef struct __attribute__((packed)) {
+    Header frame_header;
+    uint16_t cmd_id;
+    Referee::InterStudentHeader student_header;
+    uint8_t radar_cmd;
+    uint16_t crc16;
+  } RadarPack;
+
   Referee();
 
   bool UIStackEmpty();
@@ -510,6 +526,8 @@ class Referee {
 
   bool UpdateUI();
 
+  bool SentryDecision();
+
   static bool AddUI(Component::UI::Ele ui_data);
   static bool AddUI(Component::UI::Del ui_data);
   static bool AddUI(Component::UI::Str ui_data);
@@ -555,6 +573,8 @@ class Referee {
 
   System::Semaphore ui_lock_ = System::Semaphore(true);
 
+  System::Semaphore sentry_lock_ = System::Semaphore(true);
+
   Data ref_data_;
 
   Data last_data_;
@@ -562,6 +582,7 @@ class Referee {
   Message::Event event_;
 
   static UIPack ui_pack_;
+  static SentryPack sentry_pack_;
 
   static Referee *self_;
 };
diff --git a/src/module/gimbal/mod_gimbal.cpp b/src/module/gimbal/mod_gimbal.cpp
index f7e76372..2b99f473 100644
--- a/src/module/gimbal/mod_gimbal.cpp
+++ b/src/module/gimbal/mod_gimbal.cpp
@@ -36,6 +36,9 @@ Gimbal::Gimbal(Param& param, float control_freq)
       case SET_AUTOPATROL:
         gimbal->SetMode(static_cast<Mode>(AUTOPATROL));
         break;
+      case SET_ATTACKED:
+        gimbal->SetMode(static_cast<Mode>(ATTACKED));
+        break;
     }
     gimbal->ctrl_lock_.Post();
   };
@@ -84,6 +87,7 @@ void Gimbal::UpdateFeedback() {
   switch (this->mode_) {
     case RELAX:
     case ABSOLUTE:
+    case ATTACKED:
       this->yaw_ = this->yaw_motor_.GetAngle() - this->param_.mech_zero.yaw;
       break;
     case AUTOPATROL:
@@ -184,6 +188,15 @@ void Gimbal::Control() {
 
       this->yaw_motor_.Control(yaw_out);
       this->pit_motor_.Control(pit_out);
+      break;
+    case ATTACKED:
+      uint8_t angle = 0; /* 待修改 */
+      this->setpoint_.eulr_.yaw =
+          this->param_.mech_zero.yaw + (this->ref_.armor_id * angle);
+      yaw_out = this->yaw_actuator_.Calculate(
+          this->setpoint_.eulr_.yaw, this->gyro_.z, this->eulr_.yaw, this->dt_);
+      this->yaw_motor_.Control(yaw_out);
+
       break;
   }
 }
@@ -212,6 +225,9 @@ void Gimbal::SetMode(Mode mode) {
   this->mode_ = mode;
 }
 
+void Gimbal::PraseRef() {
+  this->ref_.armor_id = this->raw_ref_.robot_damage.armor_id;
+}
 void Gimbal::DrawUIStatic(Gimbal* gimbal) {
   gimbal->string_.Draw("GM", Component::UI::UI_GRAPHIC_OP_ADD,
                        Component::UI::UI_GRAPHIC_LAYER_CONST,
diff --git a/src/module/gimbal/mod_gimbal.hpp b/src/module/gimbal/mod_gimbal.hpp
index 953677f2..e2532bf4 100644
--- a/src/module/gimbal/mod_gimbal.hpp
+++ b/src/module/gimbal/mod_gimbal.hpp
@@ -22,6 +22,7 @@ class Gimbal {
     RELAX, /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */
     ABSOLUTE,   /* 绝对坐标系控制，控制在空间内的绝对姿态 */
     AUTOPATROL, /* 自动巡逻模式，云台yaw轴按sin曲线进行扫描 */
+    ATTACKED,
   } Mode;
 
   enum {
@@ -38,6 +39,7 @@ class Gimbal {
     START_AUTO_AIM,
     STOP_AUTO_AIM,
     SET_AUTOPATROL,
+    SET_ATTACKED,
   } GimbalEvent;
 
   typedef struct {
@@ -66,6 +68,10 @@ class Gimbal {
     const std::vector<Component::CMD::EventMapItem> EVENT_MAP;
 
   } Param;
+  typedef struct {
+    Device::Referee::Status status;
+    float armor_id;
+  } RefForGImbal;
 
   Gimbal(Param &param, float control_freq);
 
@@ -75,6 +81,8 @@ class Gimbal {
 
   void SetMode(Mode mode);
 
+  void PraseRef();
+
   static void DrawUIStatic(Gimbal *gimbal);
 
   static void DrawUIDynamic(Gimbal *gimbal);
@@ -104,6 +112,10 @@ class Gimbal {
   Device::RMMotor yaw_motor_;
   Device::RMMotor pit_motor_;
 
+  Device::Referee::Data raw_ref_;
+
+  RefForGImbal ref_;
+
   System::Thread thread_;
 
   System::Semaphore ctrl_lock_;
diff --git a/src/robot/sentry/robot.cpp b/src/robot/sentry/robot.cpp
index fa5183f5..e8dfafdc 100644
--- a/src/robot/sentry/robot.cpp
+++ b/src/robot/sentry/robot.cpp
@@ -288,6 +288,10 @@ Robot::Sentry::Param param = {
       Component::CMD::EventMapItem{
         Device::AI::AI_AUTOPATROL,
         Module::Gimbal::SET_AUTOPATROL
+      },
+      Component::CMD::EventMapItem{
+        Device::Referee::REF_ATTACKED,
+        Module::Gimbal::SET_ATTACKED
       }
     },
 
-- 
Gitee


From 925b71dbccb09f838a09f6d6e06fbf71f756a9d5 Mon Sep 17 00:00:00 2001
From: Marine_zxy <1423266892@qq.com>
Date: Sun, 5 May 2024 20:51:06 +0800
Subject: [PATCH 05/19] =?UTF-8?q?=E5=AE=8C=E5=96=84AI=E6=8E=A7=E5=88=B6?=
 =?UTF-8?q?=E9=80=BB=E8=BE=91?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/device/ai/dev_ai.cpp           | 164 +++++++++++++++++++++--------
 src/device/ai/dev_ai.hpp           |  16 +++
 src/device/referee/dev_referee.hpp |   2 +-
 src/module/gimbal/mod_gimbal.cpp   |   2 +-
 src/module/gimbal/mod_gimbal.hpp   |   1 -
 5 files changed, 136 insertions(+), 49 deletions(-)

diff --git a/src/device/ai/dev_ai.cpp b/src/device/ai/dev_ai.cpp
index 1e73c025..1223b4ab 100644
--- a/src/device/ai/dev_ai.cpp
+++ b/src/device/ai/dev_ai.cpp
@@ -135,35 +135,64 @@ bool AI::PackRef() {
 
   return true;
 }
+
+/* 后续通信协议要改：
+ 1、电控say导航到哪里
+ 2、视觉say是否是导航状态（电控要求一个位置，如果导航到或者已经在此为止，则算法反馈一个“已完成”，表示到达目的地附近）
+ 3、建议notice分开成
+ 底盘、云台、发射三个notice，方便三路分开也避免错误置零的情况
+ */
 void AI::DecideAction() {
-  /* 后续通信协议要改：
-  1、导航到哪里
-  2、……
-  */
-
-  /* 确认复活要写！*/
-  /* 根据AI数据、事件、受击、血量来决定底盘、云台、发射机构行为模式 */
-
-  /* 底盘 */
-  /* 无导航需求：前哨、基地、机器人血量 */
-  if (0 /* 导航*/) { /* 导航 */
-    this->action_.ai_chassis = AI::Action::TO_BASE;
-  } else { /* 小陀螺 */
+  this->notice_ = this->from_host_.data.notice;
+  // 写确认复活！！！
+
+  /* 判定是否受击打 */
+  if (this->raw_ref_.robot_damage.damage_type == 0x0 && !last_damage_type_) {
+    this->is_damaged_ = true;
+  }
+  this->last_damage_type_ = this->raw_ref_.robot_damage.damage_type;
+
+  /* 判定是否可以开始导航 */
+  if (/* 到达目的地附近|| */ is_damaged_) {
+    can_start_navigation_ = false;
+  } else {
+    can_start_navigation_ = true;
+  }
+
+  /* 底盘行为*/
+  if (can_start_navigation_) {
+    if (this->ref_.outpost_hp > 50) { /* 认为哨兵&基地无敌 */
+      this->action_.ai_chassis = AI::Action::TO_OUTPOST;
+    } else {
+      if (this->ref_.hp < 50) { /* max = 400 */
+        this->action_.ai_chassis = AI::Action::TO_PATROL_AREA;
+      } else {
+        this->action_.ai_chassis = AI::Action::TO_SUPPLY;
+      }
+
+      //   this->action_.ai_chassis = AI::Action::TO_HIGHLAND;
+      //   this->action_.ai_chassis = AI::Action::TO_BASE;
+    }
+  } else { /* 导航已完成/受打击 */
     this->action_.ai_chassis = AI::Action::ROTOR;
   }
 
-  /* 云台 */
+  /* 云台行为 */
   if (this->notice_ == AI_NOTICE_AUTO_AIM) { /* 自瞄 */
     this->action_.ai_gimbal = AI::Action::AUTO_AIM;
     this->cmd_.gimbal.mode = Component::CMD::GIMBAL_ABSOLUTE_CTRL;
-  } else if (0) { /* 反击 */
+  } else if (is_damaged_) { /* 反击 */
+    this->cmd_.gimbal.mode = Component::CMD::GIMBAL_ABSOLUTE_CTRL;
     this->action_.ai_gimbal = AI::Action::AFFECTED;
   } else { /* 扫描 */
+    this->cmd_.gimbal.mode = Component::CMD::GIMBAL_ABSOLUTE_CTRL;
     this->action_.ai_gimbal = AI::Action::SCANF;
   }
-  /* 发射 */
+
+  /* 发射机构行为 */
   if (this->notice_ == AI_NOTICE_FIRE) { /* 开火 */
     this->action_.ai_launcher = AI::Action::FIRE;
+    this->notice_ = 0;
   } else {
     this->action_.ai_launcher = AI::Action::CEASEFIRE; /* 不发弹 */
   }
@@ -172,51 +201,71 @@ void AI::DecideAction() {
 
 bool AI::PackCMD() {
   /* 根据模式，来发送不同的数据 */
-  /* 数据：*/
   /* 发布控制命令 -> cmd结构体 */
-  memcpy(&(this->cmd_.gimbal.eulr), &(this->from_host_.data.gimbal),
-         sizeof(this->cmd_.gimbal.eulr));
 
+  if (!Component::CMD::Online()) {
+    return false; /* 如果遥控器不在线 */
+  }
   memcpy(&(this->cmd_.ext.extern_channel),
          &(this->from_host_.data.extern_channel),
          sizeof(this->cmd_.ext.extern_channel));
 
-  memcpy(&(this->cmd_.chassis), &(this->from_host_.data.chassis_move_vec),
-         sizeof(this->from_host_.data.chassis_move_vec));
-
-  this->notice_ = this->from_host_.data.notice;
-  /* 以上是ai的数据直接发布 */
-  memcpy(&(this->last_eulr_), &(this->cmd_.gimbal.eulr),
-         sizeof(this->cmd_.gimbal.eulr));
-
-  this->cmd_.ctrl_source = Component::CMD::CTRL_SOURCE_AI;
+  /* 控制权在AI */
+  if (Component::CMD::GetCtrlSource() == Component::CMD::CTRL_SOURCE_AI) {
+    if (this->cmd_.online) { /* AI在线 */
+      switch (this->action_.ai_chassis) {
+        case TO_BASE:
+        case TO_OUTPOST:
+        case TO_SUPPLY:
+        case TO_HIGHLAND:
+        case TO_PATROL_AREA:
+          memcpy(&(this->cmd_.chassis),
+                 &(this->from_host_.data.chassis_move_vec),
+                 sizeof(this->from_host_.data.chassis_move_vec));
+          break;
+        case ROTOR:
+          this->cmd_.chassis.x = 0;
+          this->cmd_.chassis.y = 0;
+          this->cmd_.chassis.z = 0.5;  //待修改
+          break;
+      }
 
-  this->cmd_tp_.Publish(this->cmd_);
+      switch (this->action_.ai_gimbal) {
+        case AUTO_AIM:
+          memcpy(&(this->cmd_.gimbal.eulr), &(this->from_host_.data.gimbal),
+                 sizeof(this->cmd_.gimbal.eulr));
+          break;
+        case AFFECTED:
+          this->cmd_.gimbal.eulr.yaw = 0 /* mech_zero + id * angle */;
+          this->cmd_.gimbal.eulr.pit = 0;  //待修改成合适角度
+          break;
+        case SCANF:
+          /* 待写入合适的扫描函数 */
+          break;
+      }
 
-  if (!Component::CMD::Online()) {
-    return false; /* 如果遥控器不在线 */
-  }
+      switch (this->action_.ai_launcher) {
+        case FIRE:
+          this->event_.Active(AI_FIRE_COMMAND);  //写热量控制！！最大化利用
+        case CEASEFIRE:
+          break;
+      }
 
-  /* 控制权在AI */
-  if (Component::CMD::GetCtrlSource() == Component::CMD::CTRL_SOURCE_AI) {
-    if (this->cmd_.online) {
-      if (this->notice_ == AI_NOTICE_AUTO_AIM) {
-        this->cmd_.gimbal.mode = Component::CMD::GIMBAL_ABSOLUTE_CTRL;
-        this->event_.Active(AI_FIND_TARGET);
-      } else if (this->notice_ == AI_NOTICE_FIRE) {
-        this->cmd_.gimbal.mode = Component::CMD::GIMBAL_ABSOLUTE_CTRL;
-        this->event_.Active(AI_FIND_TARGET);
-        this->event_.Active(AI_FIRE_COMMAND);
-      } else {
-        this->event_.Active(AI_AUTOPATROL);
+      switch (this->action_.ai_to_referee) {  //需要和referee.cpp产生联系
+        case CONFIRM_RESURRECTION:
+        case EXCHANGE_BULLETS:
+        case EXCHANGE_HP:
+          break;
       }
     } else {
-      this->event_.Active(AI_OFFLINE);
+      this->event_.Active(AI_OFFLINE);  //这个逻辑是对的
     }
   }
 
+  // memcpy(&(this->last_eulr_), &(this->cmd_.gimbal.eulr),
+  //        sizeof(this->cmd_.gimbal.eulr));
   this->notice_ = 0;
-
+  this->cmd_tp_.Publish(this->cmd_);
   return true;
 }
 
@@ -283,4 +332,27 @@ void AI::PraseRef() {
     default:
       this->ref_.robot_id = AI_ARM_INFANTRY;
   }
+
+  if (this->raw_ref_.robot_status.robot_id < 100) {
+    this->ref_.base_hp = this->raw_ref_.game_robot_hp.red_base;
+    this->ref_.outpost_hp = this->raw_ref_.game_robot_hp.red_outpose;
+    this->ref_.own_virtual_shield_value =
+        this->raw_ref_.field_event.virtual_shield_value;
+  } else {
+    this->ref_.base_hp = this->raw_ref_.game_robot_hp.blue_base;
+    this->ref_.outpost_hp = this->raw_ref_.game_robot_hp.blue_outpose;
+    this->ref_.own_virtual_shield_value =
+        this->raw_ref_.field_event.virtual_shield_value;
+  }
+  this->ref_.coin_num = this->raw_ref_.bullet_remain.coin_remain;
+  this->ref_.pos_x = this->raw_ref_.robot_pos.x;
+  this->ref_.pos_y = this->raw_ref_.robot_pos.y;
+  this->ref_.pos_angle = this->raw_ref_.robot_pos.angle;
+
+  this->ref_.target_pos_x = this->raw_ref_.client_map.position_x;
+  this->ref_.target_pos_y = this->raw_ref_.client_map.position_y;
+
+  if (this->raw_ref_.robot_damage.damage_type == 0) {
+    this->ref_.damaged_armor_id = this->raw_ref_.robot_damage.armor_id;
+  }
 }
diff --git a/src/device/ai/dev_ai.hpp b/src/device/ai/dev_ai.hpp
index f9f9fbba..3c65f1e3 100644
--- a/src/device/ai/dev_ai.hpp
+++ b/src/device/ai/dev_ai.hpp
@@ -29,6 +29,18 @@ class AI {
     uint32_t ball_speed;
     uint32_t max_hp;
     uint32_t hp;
+
+    uint16_t base_hp;
+    uint16_t outpost_hp;
+    uint16_t bullet_num;
+    uint16_t coin_num;
+    uint8_t own_virtual_shield_value;
+    float pos_x;
+    float pos_y;
+    float pos_angle;
+    float target_pos_x;
+    float target_pos_y;
+    uint8_t damaged_armor_id;
   } RefForAI;
 
   typedef enum {
@@ -93,12 +105,16 @@ class AI {
 
   uint8_t notice_;
 
+  bool can_start_navigation_ = 0;
+
   struct {
     RefereePckage ref{};
     MCUPckage mcu{};
   } to_host_;
 
   RefForAI ref_{};
+  uint8_t last_damage_type_;
+  bool is_damaged_;
 
   System::Thread thread_;
 
diff --git a/src/device/referee/dev_referee.hpp b/src/device/referee/dev_referee.hpp
index 6780b859..7523e620 100644
--- a/src/device/referee/dev_referee.hpp
+++ b/src/device/referee/dev_referee.hpp
@@ -1,6 +1,6 @@
 /*
   裁判系统抽象。
-  2024_03_21 已修改。
+  2024_05_04 已更新至1.6.2版。
 */
 
 #pragma once
diff --git a/src/module/gimbal/mod_gimbal.cpp b/src/module/gimbal/mod_gimbal.cpp
index 2b99f473..0e851b88 100644
--- a/src/module/gimbal/mod_gimbal.cpp
+++ b/src/module/gimbal/mod_gimbal.cpp
@@ -190,7 +190,7 @@ void Gimbal::Control() {
       this->pit_motor_.Control(pit_out);
       break;
     case ATTACKED:
-      uint8_t angle = 0; /* 待修改 */
+      float angle = 0; /* 待修改 */
       this->setpoint_.eulr_.yaw =
           this->param_.mech_zero.yaw + (this->ref_.armor_id * angle);
       yaw_out = this->yaw_actuator_.Calculate(
diff --git a/src/module/gimbal/mod_gimbal.hpp b/src/module/gimbal/mod_gimbal.hpp
index e2532bf4..1474f041 100644
--- a/src/module/gimbal/mod_gimbal.hpp
+++ b/src/module/gimbal/mod_gimbal.hpp
@@ -113,7 +113,6 @@ class Gimbal {
   Device::RMMotor pit_motor_;
 
   Device::Referee::Data raw_ref_;
-
   RefForGImbal ref_;
 
   System::Thread thread_;
-- 
Gitee


From 82f94cf4075ae63d00c8307ce7fcc6bc24bda1fd Mon Sep 17 00:00:00 2001
From: Marine_zxy <1423266892@qq.com>
Date: Mon, 6 May 2024 21:46:25 +0800
Subject: [PATCH 06/19] =?UTF-8?q?=E5=AE=8C=E5=96=84=E5=93=A8=E5=85=B5?=
 =?UTF-8?q?=E8=A1=8C=E4=B8=BA=E5=86=B3=E7=AD=96=E9=80=BB=E8=BE=91=E4=B8=8E?=
 =?UTF-8?q?=E6=8E=A7=E5=88=B6?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/device/ai/dev_ai.cpp           | 54 +++++++++++++++++++------
 src/device/ai/dev_ai.hpp           | 41 +++++++++++++++----
 src/device/referee/dev_referee.cpp | 15 +++++--
 src/device/referee/dev_referee.hpp | 13 ++++--
 src/module/chassis/mod_chassis.cpp |  8 ++--
 src/module/chassis/mod_chassis.hpp |  7 +++-
 src/module/gimbal/mod_gimbal.cpp   | 63 +++++-------------------------
 src/module/gimbal/mod_gimbal.hpp   | 24 ++++++------
 src/robot/sentry/robot.cpp         | 59 ++++++----------------------
 9 files changed, 139 insertions(+), 145 deletions(-)

diff --git a/src/device/ai/dev_ai.cpp b/src/device/ai/dev_ai.cpp
index 1223b4ab..2e63cf1a 100644
--- a/src/device/ai/dev_ai.cpp
+++ b/src/device/ai/dev_ai.cpp
@@ -34,30 +34,34 @@ AI::AI()
     auto quat_sub =
         Message::Subscriber<Component::Type::Quaternion>("imu_quat");
     auto ref_sub = Message::Subscriber<Device::Referee::Data>("referee");
+    auto gimbal_sub = Message::Subscriber<GimbalData>("gimbal_data");
 
     while (1) {
-      /* 接收指令 */
-      ai->StartRecv();
+      /* 接收云台数据 */
+      gimbal_sub.DumpData(ai->gimbal_data_);
 
+      /* 接收裁判系统数据 */
+      if (ref_sub.DumpData(ai->raw_ref_)) {
+        ai->PraseRef();
+        ai->PackRef();
+      }
+      /* 接收上位机数据 */
+      ai->StartRecv();
       if (ai->data_ready_.Wait(0)) {
         ai->PraseHost();
       } else {
         ai->Offline();
       }
 
-      /* 发布控制命令 */
+      /* 决策与命令发布 */
       ai->DecideAction();
       ai->PackCMD();
+      ai->ai_tp_.Publish(ai->cmd_for_ref_);
 
       /* 发送数据到上位机 */
       quat_sub.DumpData(ai->quat_);
       ai->PackMCU();
 
-      if (ref_sub.DumpData(ai->raw_ref_)) {
-        ai->PraseRef();
-        ai->PackRef();
-      }
-
       ai->StartTrans();
 
       System::Thread::Sleep(2);
@@ -135,7 +139,7 @@ bool AI::PackRef() {
 
   return true;
 }
-
+void AI::GetGimbalData() {}
 /* 后续通信协议要改：
  1、电控say导航到哪里
  2、视觉say是否是导航状态（电控要求一个位置，如果导航到或者已经在此为止，则算法反馈一个“已完成”，表示到达目的地附近）
@@ -144,7 +148,6 @@ bool AI::PackRef() {
  */
 void AI::DecideAction() {
   this->notice_ = this->from_host_.data.notice;
-  // 写确认复活！！！
 
   /* 判定是否受击打 */
   if (this->raw_ref_.robot_damage.damage_type == 0x0 && !last_damage_type_) {
@@ -196,7 +199,15 @@ void AI::DecideAction() {
   } else {
     this->action_.ai_launcher = AI::Action::CEASEFIRE; /* 不发弹 */
   }
-  /* 裁判系统：确认复活、购买发弹量、购买血量 */
+
+  /* 裁判系统：确认复活、购买发弹量 */
+  if (this->ref_.hp == 0) {
+    this->action_.ai_to_referee = CONFIRM_RESURRECTION;
+  } else if (this->ref_.bullet_num == 0 && this->ref_.coin_num > 50) {
+    this->action_.ai_to_referee = EXCHANGE_BULLETS;
+  } else {
+    this->action_.ai_to_referee = NOTHING;
+  }
 }
 
 bool AI::PackCMD() {
@@ -236,10 +247,14 @@ bool AI::PackCMD() {
                  sizeof(this->cmd_.gimbal.eulr));
           break;
         case AFFECTED:
-          this->cmd_.gimbal.eulr.yaw = 0 /* mech_zero + id * angle */;
+          this->cmd_.gimbal.eulr.yaw =
+              this->gimbal_data_.mech_zero.yaw +
+              this->ref_.damaged_armor_id * (M_2PI / 4.0);
           this->cmd_.gimbal.eulr.pit = 0;  //待修改成合适角度
           break;
         case SCANF:
+          this->cmd_.gimbal.eulr.yaw = 0;
+          this->cmd_.gimbal.eulr.pit = 0;
           /* 待写入合适的扫描函数 */
           break;
       }
@@ -253,10 +268,23 @@ bool AI::PackCMD() {
 
       switch (this->action_.ai_to_referee) {  //需要和referee.cpp产生联系
         case CONFIRM_RESURRECTION:
+          cmd_for_ref_.confirm_resurrection = 1;
+          cmd_for_ref_.buy_resurrection = 0;
+          cmd_for_ref_.buy_bullet_num = this->last_buy_bullet_num_;
+          cmd_for_ref_.remote_buy_bullet_times = 0;
+          cmd_for_ref_.romote_buy_hp_times = 0;
+          break;
         case EXCHANGE_BULLETS:
-        case EXCHANGE_HP:
+          cmd_for_ref_.confirm_resurrection = 0;
+          cmd_for_ref_.buy_resurrection = 0;
+          cmd_for_ref_.buy_bullet_num = this->last_buy_bullet_num_ + 50;
+          cmd_for_ref_.remote_buy_bullet_times = 0;
+          cmd_for_ref_.romote_buy_hp_times = 0;
+          break;
+        case NOTHING:
           break;
       }
+      this->last_buy_bullet_num_ = cmd_for_ref_.buy_bullet_num;
     } else {
       this->event_.Active(AI_OFFLINE);  //这个逻辑是对的
     }
diff --git a/src/device/ai/dev_ai.hpp b/src/device/ai/dev_ai.hpp
index 3c65f1e3..bd601d23 100644
--- a/src/device/ai/dev_ai.hpp
+++ b/src/device/ai/dev_ai.hpp
@@ -43,10 +43,15 @@ class AI {
     uint8_t damaged_armor_id;
   } RefForAI;
 
+  typedef struct {
+    float yaw;
+    Component::Type::Eulr mech_zero;
+  } GimbalData;
+
   typedef enum {
     AI_OFFLINE = 128,
-    AI_FIND_TARGET,
-    AI_AUTOPATROL,
+    // AI_FIND_TARGET,
+    // AI_AUTOPATROL,
     AI_TURN,
     AI_FIRE_COMMAND,
   } AIControlData;
@@ -65,11 +70,20 @@ class AI {
     CEASEFIRE, /* 停火 */
     FIRE,
 
+    NOTHING,
     CONFIRM_RESURRECTION, /* 确认复活 */
     EXCHANGE_BULLETS,     /* 兑换弹丸 */
-    EXCHANGE_HP,          /* 兑换血量 */
+    /* 其他行为暂不考虑 */
   } Action;
 
+  typedef struct __attribute__((packed)) {
+    uint32_t confirm_resurrection : 1;
+    uint32_t buy_resurrection : 1;
+    uint32_t buy_bullet_num : 11;
+    uint32_t remote_buy_bullet_times : 4;
+    uint32_t romote_buy_hp_times : 4;
+    uint32_t res : 11;
+  } SentryDecisionData;
   typedef struct {
     uint8_t ai_chassis;    /* 小陀螺、导航 */
     uint8_t ai_gimbal;     /* 扫描、自瞄、反击 */
@@ -94,6 +108,8 @@ class AI {
 
   bool PackCMD();
 
+  void GetGimbalData();
+
   void DecideAction();
 
  private:
@@ -128,11 +144,20 @@ class AI {
 
   AICtrlAction action_;
 
-  struct {
-    float yaw; /* 偏航角（Yaw angle） */
-    float pit; /* 俯仰角（Pitch angle） */
-    float rol; /* 翻滚角（Roll angle） */
-  } last_eulr_;
+  Component::Type::Eulr mech_zero_;
+
+  GimbalData gimbal_data_;
+
+  SentryDecisionData cmd_for_ref_;
+  uint8_t last_buy_bullet_num_ = 0;
+
+  Message::Topic<SentryDecisionData> ai_tp_ =
+      Message::Topic<SentryDecisionData>("sentry_cmd_for_ref");
+  // struct {
+  //   float yaw; /* 偏航角（Yaw angle） */
+  //   float pit; /* 俯仰角（Pitch angle） */
+  //   float rol; /* 翻滚角（Roll angle） */
+  // } last_eulr_;
 
   Component::Type::Quaternion quat_{};
   Device::Referee::Data raw_ref_{};
diff --git a/src/device/referee/dev_referee.cpp b/src/device/referee/dev_referee.cpp
index 33949583..78080384 100644
--- a/src/device/referee/dev_referee.cpp
+++ b/src/device/referee/dev_referee.cpp
@@ -99,9 +99,16 @@ Referee::Referee() : event_(Message::Event::FindEvent("cmd_event")) {
                             System::Thread::REALTIME);
   auto ref_trans_thread = [](Referee *ref) {
     uint32_t last_online_time = bsp_time_get_ms();
-
+    auto ai_sub = Message::Subscriber<SentryDecisionData>("sentry_cmd_for_ref");
     while (1) {
-      ref->UpdateUI(); /* 更新UI，即机器人to裁判系统 */
+      ai_sub.DumpData(ref->sentry_pack_.sentry_cmd);
+      ref->UpdateUI(); /* 更新UI */
+
+      if (ref->ref_data_.robot_status.robot_id == REF_BOT_RED_SENTRY ||
+          ref->ref_data_.robot_status.robot_id == REF_BOT_BLU_SENTRY) {
+        ref->SentryDecision(); /* 发布哨兵决策 */
+      }
+
       ref->trans_thread_.SleepUntil(40, last_online_time);
     }
   };
@@ -496,7 +503,7 @@ bool Referee::SentryDecision() {
 
   this->sentry_pack_.cmd_id = REF_CMD_ID_INTER_STUDENT;  // 0x0301
 
-  this->sentry_pack_.sentry_cmd = 1; /* 待完善 */
+  // this->sentry_pack_.sentry_cmd 经过topic获得数据
 
   SetUIHeader(this->sentry_pack_.student_header, REF_STDNT_CMD_ID_SENTRY_CMD,
               static_cast<RobotID>(this->ref_data_.robot_status.robot_id));
@@ -507,6 +514,8 @@ bool Referee::SentryDecision() {
   bsp_uart_transmit(BSP_UART_REF,
                     reinterpret_cast<uint8_t *>(&this->sentry_pack_), pack_size,
                     false);
+  this->sentry_lock_.Post();
+  this->packet_sent_.Post();
   return true;
 }
 
diff --git a/src/device/referee/dev_referee.hpp b/src/device/referee/dev_referee.hpp
index 7523e620..0a43349d 100644
--- a/src/device/referee/dev_referee.hpp
+++ b/src/device/referee/dev_referee.hpp
@@ -497,12 +497,19 @@ class Referee {
       Referee::InterStudentHeader student_header;  //字命令、发送者、接受者
     } raw;
   };
-
+  typedef struct __attribute__((packed)) {
+    uint32_t confirm_resurrection : 1;
+    uint32_t buy_resurrection : 1;
+    uint32_t buy_bullet_num : 11;
+    uint32_t remote_buy_bullet_times : 4;
+    uint32_t romote_buy_hp_times : 4;
+    uint32_t res : 11;
+  } SentryDecisionData;
   typedef struct __attribute__((packed)) {
     Header frame_header;  // 0x0301
     uint16_t cmd_id;
     Referee::InterStudentHeader student_header;  //含0x0102
-    uint32_t sentry_cmd;
+    SentryDecisionData sentry_cmd;
     uint16_t crc16;
   } SentryPack;
 
@@ -582,7 +589,7 @@ class Referee {
   Message::Event event_;
 
   static UIPack ui_pack_;
-  static SentryPack sentry_pack_;
+  SentryPack sentry_pack_;
 
   static Referee *self_;
 };
diff --git a/src/module/chassis/mod_chassis.cpp b/src/module/chassis/mod_chassis.cpp
index f4c8c190..1913e92a 100644
--- a/src/module/chassis/mod_chassis.cpp
+++ b/src/module/chassis/mod_chassis.cpp
@@ -95,7 +95,7 @@ Chassis<Motor, MotorParam>::Chassis(Param& param, float control_freq)
     auto cmd_sub =
         Message::Subscriber<Component::CMD::ChassisCMD>("cmd_chassis");
 
-    auto yaw_sub = Message::Subscriber<float>("chassis_yaw");
+    auto yaw_sub = Message::Subscriber<GimbalData>("gimbal_data");
 
     auto cap_sub = Message::Subscriber<Device::Cap::Info>("cap_info");
 
@@ -105,7 +105,7 @@ Chassis<Motor, MotorParam>::Chassis(Param& param, float control_freq)
       /* 读取控制指令、电容、裁判系统、电机反馈 */
       cmd_sub.DumpData(chassis->cmd_);
       raw_ref_sub.DumpData(chassis->raw_ref_);
-      yaw_sub.DumpData(chassis->yaw_);
+      yaw_sub.DumpData(chassis->gimbal_data_);
       cap_sub.DumpData(chassis->cap_);
 
       /* 更新反馈值 */
@@ -163,7 +163,7 @@ void Chassis<Motor, MotorParam>::Control() {
     case Chassis::FOLLOW_GIMBAL: /* 按照云台方向换算运动向量
                                   */
     case Chassis::ROTOR: {
-      float beta = this->yaw_;
+      float beta = this->gimbal_data_.yaw;
       float cos_beta = cosf(beta);
       float sin_beta = sinf(beta);
       this->move_vec_.vx = cos_beta * this->cmd_.x - sin_beta * this->cmd_.y;
@@ -185,7 +185,7 @@ void Chassis<Motor, MotorParam>::Control() {
     case Chassis::FOLLOW_GIMBAL: /* 跟随模式通过PID控制使车头跟随云台
                                   */
       this->move_vec_.wz =
-          this->follow_pid_.Calculate(0.0f, this->yaw_, this->dt_);
+          this->follow_pid_.Calculate(0.0f, this->gimbal_data_.yaw, this->dt_);
       break;
 
     case Chassis::ROTOR: { /* 小陀螺模式使底盘以一定速度旋转
diff --git a/src/module/chassis/mod_chassis.hpp b/src/module/chassis/mod_chassis.hpp
index f5f435c4..b5aa6b5e 100644
--- a/src/module/chassis/mod_chassis.hpp
+++ b/src/module/chassis/mod_chassis.hpp
@@ -43,6 +43,11 @@ class Chassis {
     SET_MODE_INDENPENDENT,
   } ChassisEvent;
 
+  typedef struct {
+    float yaw;
+    Component::Type::Eulr mech_zero;
+  } GimbalData;
+
   /* 底盘参数的结构体，包含所有初始Component化用的参数，通常是const，存好几组 */
   typedef struct Param {
     Component::Mixer::Mode type =
@@ -121,7 +126,7 @@ class Chassis {
 
   System::Semaphore ctrl_lock_;
 
-  float yaw_;
+  GimbalData gimbal_data_;
   Device::Referee::Data raw_ref_;
   Component::CMD::ChassisCMD cmd_;
 
diff --git a/src/module/gimbal/mod_gimbal.cpp b/src/module/gimbal/mod_gimbal.cpp
index 0e851b88..bf3bd2f4 100644
--- a/src/module/gimbal/mod_gimbal.cpp
+++ b/src/module/gimbal/mod_gimbal.cpp
@@ -33,12 +33,6 @@ Gimbal::Gimbal(Param& param, float control_freq)
       case STOP_AUTO_AIM:
         Component::CMD::SetCtrlSource(Component::CMD::CTRL_SOURCE_RC);
         break;
-      case SET_AUTOPATROL:
-        gimbal->SetMode(static_cast<Mode>(AUTOPATROL));
-        break;
-      case SET_ATTACKED:
-        gimbal->SetMode(static_cast<Mode>(ATTACKED));
-        break;
     }
     gimbal->ctrl_lock_.Post();
   };
@@ -66,7 +60,7 @@ Gimbal::Gimbal(Param& param, float control_freq)
       gimbal->Control();
       gimbal->ctrl_lock_.Post();
 
-      gimbal->yaw_tp_.Publish(gimbal->yaw_);
+      gimbal->yaw_tp_.Publish(gimbal->gimbal_data_);
 
       /* 运行结束，等待下一次唤醒 */
       gimbal->thread_.SleepUntil(2, last_online_time);
@@ -87,19 +81,11 @@ void Gimbal::UpdateFeedback() {
   switch (this->mode_) {
     case RELAX:
     case ABSOLUTE:
-    case ATTACKED:
-      this->yaw_ = this->yaw_motor_.GetAngle() - this->param_.mech_zero.yaw;
-      break;
-    case AUTOPATROL:
-      this->yaw_ = this->yaw_motor_.GetAngle() - this->param_.mech_zero.yaw -
-                   this->param_.patrol_range *
-                       sinf(this->param_.patrol_omega *
-                            static_cast<float>(bsp_time_get_ms() -
-                                               autopatrol_start_time_) /
-                            1000.0f);
-
+      this->gimbal_data_.yaw =
+          this->yaw_motor_.GetAngle() - this->param_.mech_zero.yaw;
       break;
   }
+  this->gimbal_data_.mech_zero = this->param_.mech_zero;
 }
 
 void Gimbal::Control() {
@@ -153,7 +139,6 @@ void Gimbal::Control() {
   /* 控制相关逻辑 */
   float yaw_out = 0;
   float pit_out = 0;
-  float autopatrol_yaw = 0;
   switch (this->mode_) {
     case RELAX:
       this->yaw_motor_.Relax();
@@ -170,33 +155,6 @@ void Gimbal::Control() {
       this->yaw_motor_.Control(yaw_out);
       this->pit_motor_.Control(pit_out);
 
-      break;
-
-    case AUTOPATROL:
-      /* 以sin变化左右摆头 */
-      autopatrol_yaw = this->setpoint_.eulr_.yaw +
-                       this->param_.patrol_range *
-                           sinf(this->param_.patrol_omega *
-                                static_cast<float>(bsp_time_get_ms() -
-                                                   autopatrol_start_time_) /
-                                1000.0f);
-
-      yaw_out = this->yaw_actuator_.Calculate(autopatrol_yaw, this->gyro_.z,
-                                              this->eulr_.yaw, this->dt_);
-      pit_out = this->pit_actuator_.Calculate(
-          this->param_.patrol_hight, this->gyro_.x, this->eulr_.pit, this->dt_);
-
-      this->yaw_motor_.Control(yaw_out);
-      this->pit_motor_.Control(pit_out);
-      break;
-    case ATTACKED:
-      float angle = 0; /* 待修改 */
-      this->setpoint_.eulr_.yaw =
-          this->param_.mech_zero.yaw + (this->ref_.armor_id * angle);
-      yaw_out = this->yaw_actuator_.Calculate(
-          this->setpoint_.eulr_.yaw, this->gyro_.z, this->eulr_.yaw, this->dt_);
-      this->yaw_motor_.Control(yaw_out);
-
       break;
   }
 }
@@ -218,16 +176,13 @@ void Gimbal::SetMode(Mode mode) {
     }
   }
 
-  if (mode == AUTOPATROL) {
-    autopatrol_start_time_ = bsp_time_get_ms();
-  }
-
   this->mode_ = mode;
 }
 
 void Gimbal::PraseRef() {
   this->ref_.armor_id = this->raw_ref_.robot_damage.armor_id;
 }
+
 void Gimbal::DrawUIStatic(Gimbal* gimbal) {
   gimbal->string_.Draw("GM", Component::UI::UI_GRAPHIC_OP_ADD,
                        Component::UI::UI_GRAPHIC_LAYER_CONST,
@@ -297,9 +252,9 @@ void Gimbal::DrawUIStatic(Gimbal* gimbal) {
       static_cast<uint16_t>(Device::Referee::UIGetWidth() * 0.4f),
       static_cast<uint16_t>(Device::Referee::UIGetHeight() * 0.2f),
       static_cast<uint16_t>(Device::Referee::UIGetWidth() * 0.4f +
-                            -sinf(gimbal->yaw_) * 44),
+                            -sinf(gimbal->gimbal_data_.yaw) * 44),
       static_cast<uint16_t>(Device::Referee::UIGetHeight() * 0.2f +
-                            cosf(gimbal->yaw_) * 44));
+                            cosf(gimbal->gimbal_data_.yaw) * 44));
   Device::Referee::AddUI(gimbal->line_);
 }
 
@@ -352,8 +307,8 @@ void Gimbal::DrawUIDynamic(Gimbal* gimbal) {
       static_cast<uint16_t>(Device::Referee::UIGetWidth() * 0.4f),
       static_cast<uint16_t>(Device::Referee::UIGetHeight() * 0.2f),
       static_cast<uint16_t>(Device::Referee::UIGetWidth() * 0.4f +
-                            -sinf(gimbal->yaw_) * 44),
+                            -sinf(gimbal->gimbal_data_.yaw) * 44),
       static_cast<uint16_t>(Device::Referee::UIGetHeight() * 0.2f +
-                            cosf(gimbal->yaw_) * 44));
+                            cosf(gimbal->gimbal_data_.yaw) * 44));
   Device::Referee::AddUI(gimbal->line_);
 }
diff --git a/src/module/gimbal/mod_gimbal.hpp b/src/module/gimbal/mod_gimbal.hpp
index 1474f041..9d1d6d92 100644
--- a/src/module/gimbal/mod_gimbal.hpp
+++ b/src/module/gimbal/mod_gimbal.hpp
@@ -20,9 +20,7 @@ class Gimbal {
   /* 云台运行模式 */
   typedef enum {
     RELAX, /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */
-    ABSOLUTE,   /* 绝对坐标系控制，控制在空间内的绝对姿态 */
-    AUTOPATROL, /* 自动巡逻模式，云台yaw轴按sin曲线进行扫描 */
-    ATTACKED,
+    ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */
   } Mode;
 
   enum {
@@ -38,9 +36,11 @@ class Gimbal {
     SET_MODE_ABSOLUTE,
     START_AUTO_AIM,
     STOP_AUTO_AIM,
-    SET_AUTOPATROL,
-    SET_ATTACKED,
   } GimbalEvent;
+  typedef struct {
+    float yaw;
+    Component::Type::Eulr mech_zero;
+  } GimbalData;
 
   typedef struct {
     Component::SecOrderFunction::Param ff; /* PITCH前馈 */
@@ -54,9 +54,10 @@ class Gimbal {
 
     Component::Type::Eulr mech_zero;
 
-    float patrol_range;
-    float patrol_omega;
-    float patrol_hight;
+    float scanf_yaw_rate;
+    float scanf_pit_center;
+    float scanf_pit_range;
+    float scanf_pit_omega;
 
     struct {
       Component::Type::CycleValue pitch_max;
@@ -94,8 +95,6 @@ class Gimbal {
 
   float dt_ = 0.0f;
 
-  uint32_t autopatrol_start_time_ = 0;
-
   Param param_;
 
   Gimbal::Mode mode_ = RELAX; /* 云台模式 */
@@ -119,9 +118,10 @@ class Gimbal {
 
   System::Semaphore ctrl_lock_;
 
-  Message::Topic<float> yaw_tp_ = Message::Topic<float>("chassis_yaw");
+  Message::Topic<GimbalData> yaw_tp_ =
+      Message::Topic<GimbalData>("gimbal_data");
 
-  float yaw_;
+  GimbalData gimbal_data_;
 
   Component::UI::String string_;
 
diff --git a/src/robot/sentry/robot.cpp b/src/robot/sentry/robot.cpp
index e8dfafdc..1892d2a6 100644
--- a/src/robot/sentry/robot.cpp
+++ b/src/robot/sentry/robot.cpp
@@ -29,31 +29,19 @@ Robot::Sentry::Param param = {
       },
       Component::CMD::EventMapItem{
         Device::DR16::DR16_SW_L_POS_TOP,
-        Module::RMChassis::SET_MODE_RELAX,
-      },
-      Component::CMD::EventMapItem{
-        Device::DR16::DR16_SW_L_POS_MID,/* 模拟找到目标模式，云台绝对 */
-        Module::RMChassis::SET_MODE_FOLLOW,
-      },
-      Component::CMD::EventMapItem{
-        Device::DR16::DR16_SW_L_POS_BOT,/* 模拟未找到目标，巡逻模式 */
-        Module::RMChassis::SET_MODE_ROTOR,
-      },
-      Component::CMD::EventMapItem{
-        Device::AI::AI_OFFLINE,
-        Module::RMChassis::SET_MODE_ROTOR
+        Module::RMChassis::SET_MODE_RELAX
       },
       Component::CMD::EventMapItem{
-        Device::AI::AI_FIND_TARGET,
-        Module::RMChassis::SET_MODE_ROTOR
+        Device::DR16::DR16_SW_L_POS_MID,
+        Module::RMChassis::SET_MODE_FOLLOW
       },
       Component::CMD::EventMapItem{
-        Device::AI::AI_AUTOPATROL,
+        Device::DR16::DR16_SW_L_POS_BOT,
         Module::RMChassis::SET_MODE_ROTOR
       },
       Component::CMD::EventMapItem{
-        Device::AI::AI_TURN,
-        Module::RMChassis::SET_MODE_ROTOR
+        Device::AI::AI_OFFLINE,
+        Module::RMChassis::SET_MODE_RELAX
       }
     },
 
@@ -251,9 +239,10 @@ Robot::Sentry::Param param = {
       .rol = 0.0f,
     },
 
-    .patrol_range = 0.4f,
-    .patrol_omega = 2.0f,
-    .patrol_hight = 6.0,
+    .scanf_yaw_rate = 2.0f,
+    .scanf_pit_center = 0.6f,
+    .scanf_pit_range = 0.2f,/* 都待修改 */
+    .scanf_pit_omega = 6.0f,
 
     .limit = {
       .pitch_max = 0.60f,
@@ -270,28 +259,12 @@ Robot::Sentry::Param param = {
         Module::Gimbal::SET_MODE_ABSOLUTE
       },
       Component::CMD::EventMapItem{
-        Device::DR16::DR16_SW_L_POS_MID,/* 模拟找到目标模式，云台绝对 */
+        Device::DR16::DR16_SW_R_POS_MID,
         Module::Gimbal::SET_MODE_ABSOLUTE
       },
       Component::CMD::EventMapItem{
-        Device::DR16::DR16_SW_L_POS_BOT,/* 模拟未找到目标，巡逻模式 */
-        Module::Gimbal::SET_AUTOPATROL
-      },
-      Component::CMD::EventMapItem{
-        Device::AI::AI_OFFLINE,
-        Module::Gimbal::SET_AUTOPATROL
-      },
-      Component::CMD::EventMapItem{
-        Device::AI::AI_FIND_TARGET,
+        Device::DR16::DR16_SW_R_POS_BOT,
         Module::Gimbal::SET_MODE_ABSOLUTE
-      },
-      Component::CMD::EventMapItem{
-        Device::AI::AI_AUTOPATROL,
-        Module::Gimbal::SET_AUTOPATROL
-      },
-      Component::CMD::EventMapItem{
-        Device::Referee::REF_ATTACKED,
-        Module::Gimbal::SET_ATTACKED
       }
     },
 
@@ -416,10 +389,6 @@ Robot::Sentry::Param param = {
       Component::CMD::EventMapItem{
         Device::DR16::DR16_SW_R_POS_BOT,/* 模拟未找到目标，巡逻模式 */
         Module::Launcher::LAUNCHER_START_FIRE
-      },
-      Component::CMD::EventMapItem{
-        Device::AI::AIControlData::AI_AUTOPATROL,
-        Module::Launcher::CHANGE_FIRE_MODE_RELAX
       },
         Component::CMD::EventMapItem{
         Device::AI::AIControlData::AI_FIRE_COMMAND,
@@ -554,10 +523,6 @@ Robot::Sentry::Param param = {
       Component::CMD::EventMapItem{
         Device::DR16::DR16_SW_R_POS_BOT,/* 模拟未找到目标，巡逻模式 */
         Module::Launcher::LAUNCHER_START_FIRE
-      },
-      Component::CMD::EventMapItem{
-        Device::AI::AIControlData::AI_AUTOPATROL,
-        Module::Launcher::CHANGE_FIRE_MODE_RELAX
       },
         Component::CMD::EventMapItem{
         Device::AI::AIControlData::AI_FIRE_COMMAND,
-- 
Gitee


From b178783e755ac585a0735173601ce3aacbe90b43 Mon Sep 17 00:00:00 2001
From: Marine_zxy <1423266892@qq.com>
Date: Wed, 8 May 2024 20:30:21 +0800
Subject: [PATCH 07/19] =?UTF-8?q?=E6=96=B0=E5=A2=9E=E5=85=A8=E5=90=91?=
 =?UTF-8?q?=E8=BD=AE=E6=9C=BA=E5=99=A8=E4=BA=BA=E5=92=8CMIT=E7=94=B5?=
 =?UTF-8?q?=E6=9C=BA=E4=BA=91=E5=8F=B0?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 hw/bsp/rm-c/config/omni_infantry.config  | 157 ++++++++
 src/module/mit_gimbal/Kconfig            |   0
 src/module/mit_gimbal/info.cmake         |   6 +
 src/module/mit_gimbal/mod_mit_gimbal.cpp | 314 ++++++++++++++++
 src/module/mit_gimbal/mod_mit_gimbal.hpp | 136 +++++++
 src/robot/omni_infantry/Kconfig          |   0
 src/robot/omni_infantry/robot.cpp        | 437 +++++++++++++++++++++++
 src/robot/omni_infantry/robot.hpp        |  48 +++
 8 files changed, 1098 insertions(+)
 create mode 100644 hw/bsp/rm-c/config/omni_infantry.config
 create mode 100644 src/module/mit_gimbal/Kconfig
 create mode 100644 src/module/mit_gimbal/info.cmake
 create mode 100644 src/module/mit_gimbal/mod_mit_gimbal.cpp
 create mode 100644 src/module/mit_gimbal/mod_mit_gimbal.hpp
 create mode 100644 src/robot/omni_infantry/Kconfig
 create mode 100644 src/robot/omni_infantry/robot.cpp
 create mode 100644 src/robot/omni_infantry/robot.hpp

diff --git a/hw/bsp/rm-c/config/omni_infantry.config b/hw/bsp/rm-c/config/omni_infantry.config
new file mode 100644
index 00000000..fba87b3c
--- /dev/null
+++ b/hw/bsp/rm-c/config/omni_infantry.config
@@ -0,0 +1,157 @@
+# CONFIG_auto_generated_config_prefix_board-MiniPC_with_canfd is not set
+# CONFIG_auto_generated_config_prefix_board-Webots is not set
+# CONFIG_auto_generated_config_prefix_board-mangopi_r818 is not set
+# CONFIG_auto_generated_config_prefix_board-dual_canfd is not set
+# CONFIG_auto_generated_config_prefix_board-node_imu is not set
+# CONFIG_auto_generated_config_prefix_board-MiniPC is not set
+# CONFIG_auto_generated_config_prefix_board-microswitch is not set
+# CONFIG_auto_generated_config_prefix_board-esp32-c3-arduino is not set
+# CONFIG_auto_generated_config_prefix_board-esp32-c3-idf is not set
+# CONFIG_auto_generated_config_prefix_board-c-mini is not set
+CONFIG_auto_generated_config_prefix_board-rm-c=y
+# CONFIG_auto_generated_config_prefix_board-f103_can is not set
+CONFIG_auto_generated_config_prefix_system-FreeRTOS=y
+# CONFIG_auto_generated_config_prefix_system-None is not set
+# CONFIG_auto_generated_config_prefix_system-Bootloader is not set
+# CONFIG_auto_generated_config_prefix_system-Linux is not set
+# CONFIG_auto_generated_config_prefix_system-Linux_Webots is not set
+
+#
+# FreeRTOS
+#
+CONFIG_INIT_TASK_STACK_DEPTH=2048
+CONFIG_FREERTOS_TIMER_TASK_STACK_DEPTH=512
+CONFIG_FREERTOS_USB_TASK_STACK_DEPTH=256
+CONFIG_FREERTOS_TERM_TASK_STACK_DEPTH=512
+# end of FreeRTOS
+
+# CONFIG_auto_generated_config_prefix_robot-dart is not set
+# CONFIG_auto_generated_config_prefix_robot-infantry is not set
+# CONFIG_auto_generated_config_prefix_robot-blink is not set
+# CONFIG_auto_generated_config_prefix_robot-sim_mecanum is not set
+# CONFIG_auto_generated_config_prefix_robot-drone_gimbal is not set
+# CONFIG_auto_generated_config_prefix_robot-hero is not set
+# CONFIG_auto_generated_config_prefix_robot-balance_infantry is not set
+# CONFIG_auto_generated_config_prefix_robot-can_to_uart is not set
+# CONFIG_auto_generated_config_prefix_robot-sentry is not set
+# CONFIG_auto_generated_config_prefix_robot-bootloader is not set
+# CONFIG_auto_generated_config_prefix_robot-udp_to_uart is not set
+# CONFIG_auto_generated_config_prefix_robot-helm_infantry is not set
+# CONFIG_auto_generated_config_prefix_robot-uart_net_config is not set
+# CONFIG_auto_generated_config_prefix_robot-engineer is not set
+# CONFIG_auto_generated_config_prefix_robot-microswitch is not set
+CONFIG_auto_generated_config_prefix_robot-omni_infantry=y
+# CONFIG_auto_generated_config_prefix_robot-canfd_to_uart is not set
+# CONFIG_auto_generated_config_prefix_robot-wearlab_imu is not set
+# CONFIG_auto_generated_config_prefix_robot-ble_net_config is not set
+# CONFIG_auto_generated_config_prefix_robot-custom_controller is not set
+# CONFIG_auto_generated_config_prefix_robot-sim_balance is not set
+
+#
+# 组件
+#
+
+#
+# 设备
+#
+CONFIG_auto_generated_config_prefix_device-imu=y
+CONFIG_DEVICE_CAN_IMU_TASK_STACK_DEPTH=256
+# CONFIG_auto_generated_config_prefix_device-blink_led is not set
+CONFIG_auto_generated_config_prefix_device-ahrs=y
+CONFIG_DEVICE_AHRS_TASK_STACK_DEPTH=256
+# CONFIG_auto_generated_config_prefix_device-mmc5603 is not set
+CONFIG_auto_generated_config_prefix_device-can=y
+CONFIG_auto_generated_config_prefix_device-motor=y
+# CONFIG_auto_generated_config_prefix_device-ina226 is not set
+CONFIG_auto_generated_config_prefix_device-dr16=y
+CONFIG_DEVICE_DR16_TASK_STACK_DEPTH=384
+# CONFIG_auto_generated_config_prefix_device-simulator is not set
+CONFIG_REF_LAUNCH_SPEED=30
+CONFIG_REF_HEAT_LIMIT_17=100
+CONFIG_REF_HEAT_LIMIT_42=100
+CONFIG_REF_POWER_LIMIT=200
+CONFIG_REF_POWER_BUFF=100
+# CONFIG_auto_generated_config_prefix_device-bq27220 is not set
+# CONFIG_auto_generated_config_prefix_device-spl06_001 is not set
+CONFIG_auto_generated_config_prefix_device-led_rgb=y
+CONFIG_DEVICE_LED_RGB_TASK_STACK_DEPTH=128
+# CONFIG_auto_generated_config_prefix_device-tof is not set
+# CONFIG_auto_generated_config_prefix_device-icm42688 is not set
+# CONFIG_auto_generated_config_prefix_device-laser is not set
+# CONFIG_auto_generated_config_prefix_device-net_config is not set
+CONFIG_auto_generated_config_prefix_device-referee=y
+CONFIG_DEVICE_REF_TRANS_TASK_STACK_DEPTH=256
+CONFIG_DEVICE_REF_RECV_TASK_STACK_DEPTH=256
+
+#
+# 裁判系统
+#
+# CONFIG_REF_VIRTUAL is not set
+# end of 裁判系统
+
+#
+# 操作手UI
+#
+CONFIG_UI_DYNAMIC_CYCLE=20
+CONFIG_UI_STATIC_CYCLE=1000
+# end of 操作手UI
+
+# CONFIG_auto_generated_config_prefix_device-mech is not set
+# CONFIG_auto_generated_config_prefix_device-microswitch is not set
+# CONFIG_auto_generated_config_prefix_device-canfd is not set
+CONFIG_auto_generated_config_prefix_device-servo=y
+CONFIG_auto_generated_config_prefix_device-cap=y
+CONFIG_DEVICE_CAP_TASK_STACK_DEPTH=256
+
+#
+# 超级电容
+#
+CONFIG_CAP_PERCENT_NO_LIM=80
+CONFIG_CAP_PERCENT_WORK=30
+CONFIG_CAP_MAX_LOAD=100
+# end of 超级电容
+
+CONFIG_auto_generated_config_prefix_device-bmi088=y
+CONFIG_DEVICE_BMI088_TASK_STACK_DEPTH=256
+# CONFIG_auto_generated_config_prefix_device-ahrs-9 is not set
+# CONFIG_auto_generated_config_prefix_device-custom_controller is not set
+# CONFIG_auto_generated_config_prefix_device-buzzer is not set
+CONFIG_auto_generated_config_prefix_device-ai=y
+CONFIG_DEVICE_AI_TASK_STACK_DEPTH=384
+
+#
+# 上位机
+#
+# CONFIG_HOST_CTRL_PRIORITY is not set
+# end of 上位机
+# end of 设备
+
+#
+# 模块
+#
+CONFIG_auto_generated_config_prefix_module-launcher=y
+CONFIG_MODULE_GIMBAL_TASK_STACK_DEPTH=512
+CONFIG_MODULE_LAUNCHER_TASK_STACK_DEPTH=384
+# CONFIG_auto_generated_config_prefix_module-uart_update is not set
+# CONFIG_auto_generated_config_prefix_module-topic_share_uart is not set
+# CONFIG_auto_generated_config_prefix_module-ore_collect is not set
+# CONFIG_auto_generated_config_prefix_module-helm_chassis is not set
+# CONFIG_auto_generated_config_prefix_module-dart_launcher is not set
+# CONFIG_auto_generated_config_prefix_module-gimbal is not set
+# CONFIG_auto_generated_config_prefix_module-engineer_chassis is not set
+# CONFIG_auto_generated_config_prefix_module-wheel_leg is not set
+# CONFIG_auto_generated_config_prefix_module-can_imu_wearlab is not set
+CONFIG_auto_generated_config_prefix_module-chassis=y
+CONFIG_MODULE_CHASSIS_TASK_STACK_DEPTH=384
+# CONFIG_auto_generated_config_prefix_module-microswitch is not set
+# CONFIG_auto_generated_config_prefix_module-balance is not set
+# CONFIG_auto_generated_config_prefix_module-dart_gimbal is not set
+# CONFIG_auto_generated_config_prefix_module-launcher_drone is not set
+# CONFIG_auto_generated_config_prefix_module-performance is not set
+# CONFIG_auto_generated_config_prefix_module-canfd_to_uart is not set
+# CONFIG_auto_generated_config_prefix_module-can_usart is not set
+# CONFIG_auto_generated_config_prefix_module-ble_net_config is not set
+# CONFIG_auto_generated_config_prefix_module-custom_controller is not set
+# CONFIG_auto_generated_config_prefix_module-can_imu is not set
+CONFIG_auto_generated_config_prefix_module-mit_gimbal=y
+# end of 模块
diff --git a/src/module/mit_gimbal/Kconfig b/src/module/mit_gimbal/Kconfig
new file mode 100644
index 00000000..e69de29b
diff --git a/src/module/mit_gimbal/info.cmake b/src/module/mit_gimbal/info.cmake
new file mode 100644
index 00000000..35463fe0
--- /dev/null
+++ b/src/module/mit_gimbal/info.cmake
@@ -0,0 +1,6 @@
+CHECK_SUB_ENABLE(MODULE_ENABLE module)
+if(${MODULE_ENABLE})
+    file(GLOB CUR_SOURCES "${SUB_DIR}/*.cpp")
+    SUB_ADD_SRC(CUR_SOURCES)
+    SUB_ADD_INC(SUB_DIR)
+endif()
\ No newline at end of file
diff --git a/src/module/mit_gimbal/mod_mit_gimbal.cpp b/src/module/mit_gimbal/mod_mit_gimbal.cpp
new file mode 100644
index 00000000..119bf203
--- /dev/null
+++ b/src/module/mit_gimbal/mod_mit_gimbal.cpp
@@ -0,0 +1,314 @@
+#include "mod_mit_gimbal.hpp"
+
+#include <comp_cmd.hpp>
+#include <comp_type.hpp>
+#include <comp_ui.hpp>
+
+#include "bsp_time.h"
+
+using namespace Module;
+
+#define GIMBAL_MAX_SPEED (M_2PI * 1.5f)
+
+Gimbal::Gimbal(Param& param, float control_freq)
+    : param_(param),
+      st_(param.st),
+      yaw_actuator_(this->param_.yaw_actr, control_freq),
+      pit_actuator_(this->param_.pit_actr, control_freq),
+      yaw_motor_(this->param_.yaw_motor, "Gimbal_Yaw"),
+      pit_motor_(this->param_.pit_motor, "Gimbal_Pitch"),
+      ctrl_lock_(true) {
+  auto event_callback = [](GimbalEvent event, Gimbal* gimbal) {
+    gimbal->ctrl_lock_.Wait(UINT32_MAX);
+
+    switch (event) {
+      case SET_MODE_RELAX:
+      case SET_MODE_ABSOLUTE:
+        gimbal->SetMode(static_cast<Mode>(event));
+        break;
+
+      case START_AUTO_AIM:
+        Component::CMD::SetCtrlSource(Component::CMD::CTRL_SOURCE_AI);
+        break;
+      case STOP_AUTO_AIM:
+        Component::CMD::SetCtrlSource(Component::CMD::CTRL_SOURCE_RC);
+        break;
+    }
+    gimbal->ctrl_lock_.Post();
+  };
+
+  Component::CMD::RegisterEvent<Gimbal*, GimbalEvent>(event_callback, this,
+                                                      this->param_.EVENT_MAP);
+
+  auto gimbal_thread = [](Gimbal* gimbal) {
+    auto eulr_sub = Message::Subscriber<Component::Type::Eulr>("imu_eulr");
+
+    auto gyro_sub = Message::Subscriber<Component::Type::Vector3>("imu_gyro");
+
+    auto cmd_sub = Message::Subscriber<Component::CMD::GimbalCMD>("cmd_gimbal");
+
+    uint32_t last_online_time = bsp_time_get_ms();
+
+    while (1) {
+      /* 读取控制指令、姿态、IMU、电机反馈 */
+      eulr_sub.DumpData(gimbal->eulr_); /* imu */
+      gyro_sub.DumpData(gimbal->gyro_); /* imu */
+      cmd_sub.DumpData(gimbal->cmd_);   /* cmd */
+
+      gimbal->ctrl_lock_.Wait(UINT32_MAX);
+      gimbal->UpdateFeedback();
+      gimbal->Control();
+      gimbal->ctrl_lock_.Post();
+
+      gimbal->yaw_tp_.Publish(gimbal->gimbal_data_);
+
+      /* 运行结束，等待下一次唤醒 */
+      gimbal->thread_.SleepUntil(2, last_online_time);
+    }
+  };
+
+  this->thread_.Create(gimbal_thread, this, "gimbal_thread",
+                       MODULE_GIMBAL_TASK_STACK_DEPTH, System::Thread::MEDIUM);
+
+  System::Timer::Create(this->DrawUIStatic, this, 2000);
+
+  System::Timer::Create(this->DrawUIDynamic, this, 60);
+}
+
+void Gimbal::UpdateFeedback() {
+  this->pit_motor_.Update();
+  this->yaw_motor_.Update();
+  switch (this->mode_) {
+    case RELAX:
+    case ABSOLUTE:
+      this->gimbal_data_.yaw =
+          this->yaw_motor_.GetAngle() - this->param_.mech_zero.yaw;
+      break;
+  }
+  this->gimbal_data_.mech_zero = this->param_.mech_zero;
+}
+
+void Gimbal::Control() {
+  this->now_ = bsp_time_get();
+  this->dt_ = TIME_DIFF(this->last_wakeup_, this->now_);
+
+  this->last_wakeup_ = this->now_;
+
+  float gimbal_pit_cmd = 0.0f;
+  float gimbal_yaw_cmd = 0.0f;
+
+  /* yaw坐标正方向与遥控器操作逻辑相反 */
+  if (this->cmd_.mode == Component::CMD::GIMBAL_RELATIVE_CTRL) {
+    gimbal_yaw_cmd = this->cmd_.eulr.yaw * this->dt_ * GIMBAL_MAX_SPEED;
+    gimbal_pit_cmd = this->cmd_.eulr.pit * this->dt_ * GIMBAL_MAX_SPEED;
+
+  } else {
+    gimbal_yaw_cmd = Component::Type::CycleValue(this->cmd_.eulr.yaw) -
+                     this->setpoint_.eulr_.yaw;
+    gimbal_pit_cmd = Component::Type::CycleValue(this->cmd_.eulr.pit) -
+                     this->setpoint_.eulr_.pit;
+  }
+
+  /* 处理yaw控制命令，软件限位  */
+  /* 某个轴max=min时不进行限位,配置文件默认不写 */
+  if (param_.limit.yaw_max != param_.limit.yaw_min) {
+    const float ENCODER_DELTA_MAX_YAW =
+        this->param_.limit.yaw_max - this->yaw_motor_.GetAngle();
+    const float ENCODER_DELTA_MIN_YAW =
+        this->param_.limit.yaw_min - this->yaw_motor_.GetAngle();
+    const float YAW_ERR = this->setpoint_.eulr_.yaw - eulr_.yaw;
+    const float DELTA_MAX_YAW = ENCODER_DELTA_MAX_YAW - YAW_ERR;
+    const float DELTA_MIN_YAW = ENCODER_DELTA_MIN_YAW - YAW_ERR;
+    clampf(&(gimbal_yaw_cmd), DELTA_MIN_YAW, DELTA_MAX_YAW);
+  }
+  this->setpoint_.eulr_.yaw += gimbal_yaw_cmd;
+
+  /* 处理pitch控制命令，软件限位 */
+  if (param_.limit.pitch_max != param_.limit.pitch_min) {
+    const float ENCODER_DELTA_MAX_PIT =
+        this->param_.limit.pitch_max - this->pit_motor_.GetAngle();
+    const float ENCODER_DELTA_MIN_PIT =
+        this->param_.limit.pitch_min - this->pit_motor_.GetAngle();
+    const float PIT_ERR = this->setpoint_.eulr_.pit - eulr_.pit;
+    const float DELTA_MAX_PIT = ENCODER_DELTA_MAX_PIT - PIT_ERR;
+    const float DELTA_MIN_PIT = ENCODER_DELTA_MIN_PIT - PIT_ERR;
+    clampf(&(gimbal_pit_cmd), DELTA_MIN_PIT, DELTA_MAX_PIT);
+  }
+  this->setpoint_.eulr_.pit += gimbal_pit_cmd;
+
+  /* 控制相关逻辑 */
+  float yaw_out = 0;
+  float pit_out = 0;
+  switch (this->mode_) {
+    case RELAX:
+      this->yaw_motor_.Relax();
+      this->pit_motor_.Relax();
+      break;
+    case ABSOLUTE:
+      /* Yaw轴角速度环参数计算 */
+      yaw_out = this->yaw_actuator_.Calculate(
+          this->setpoint_.eulr_.yaw, this->gyro_.z, this->eulr_.yaw, this->dt_);
+
+      pit_out = this->pit_actuator_.Calculate(
+          this->setpoint_.eulr_.pit, this->gyro_.x, this->eulr_.pit, this->dt_);
+
+      this->yaw_motor_.Control(yaw_out);
+      this->pit_motor_.Control(pit_out);
+
+      break;
+  }
+}
+
+void Gimbal::SetMode(Mode mode) {
+  if (mode == this->mode_) {
+    return;
+  }
+
+  /* 切换模式后重置PID和滤波器 */
+  this->pit_actuator_.Reset();
+  this->yaw_actuator_.Reset();
+
+  memcpy(&(this->setpoint_.eulr_), &(this->eulr_),
+         sizeof(this->setpoint_.eulr_)); /* 切换模式后重置设定值 */
+  if (this->mode_ == RELAX) {
+    if (mode == ABSOLUTE) {
+      this->setpoint_.eulr_.yaw = this->eulr_.yaw;
+    }
+  }
+
+  this->mode_ = mode;
+}
+
+void Gimbal::PraseRef() {
+  this->ref_.armor_id = this->raw_ref_.robot_damage.armor_id;
+}
+
+void Gimbal::DrawUIStatic(Gimbal* gimbal) {
+  gimbal->string_.Draw("GM", Component::UI::UI_GRAPHIC_OP_ADD,
+                       Component::UI::UI_GRAPHIC_LAYER_CONST,
+                       Component::UI::UI_GREEN, UI_DEFAULT_WIDTH * 10, 80,
+                       UI_CHAR_DEFAULT_WIDTH,
+                       static_cast<uint16_t>(Device::Referee::UIGetWidth() *
+                                             REF_UI_RIGHT_START_W),
+                       static_cast<uint16_t>(Device::Referee::UIGetHeight() *
+                                             REF_UI_MODE_LINE2_H),
+                       "GMBL  RELX  ABSL  RLTV");
+  Device::Referee::AddUI(gimbal->string_);
+
+  float box_pos_left = 0.0f, box_pos_right = 0.0f;
+
+  /* 更新云台模式选择框 */
+  switch (gimbal->mode_) {
+    case RELAX:
+      box_pos_left = REF_UI_MODE_OFFSET_2_LEFT;
+      box_pos_right = REF_UI_MODE_OFFSET_2_RIGHT;
+      break;
+    case ABSOLUTE:
+      if (gimbal->cmd_.mode == Component::CMD::GIMBAL_ABSOLUTE_CTRL) {
+        box_pos_left = REF_UI_MODE_OFFSET_3_LEFT;
+        box_pos_right = REF_UI_MODE_OFFSET_3_RIGHT;
+      } else {
+        box_pos_left = REF_UI_MODE_OFFSET_4_LEFT;
+        box_pos_right = REF_UI_MODE_OFFSET_4_RIGHT;
+      }
+      break;
+    default:
+      box_pos_left = 0.0f;
+      box_pos_right = 0.0f;
+      break;
+  }
+  if (box_pos_left != 0.0f && box_pos_right != 0.0f) {
+    gimbal->rectangle_.Draw(
+        "GS", Component::UI::UI_GRAPHIC_OP_ADD,
+        Component::UI::UI_GRAPHIC_LAYER_GIMBAL, Component::UI::UI_GREEN,
+        UI_DEFAULT_WIDTH,
+        static_cast<uint16_t>(Device::Referee::UIGetWidth() *
+                                  REF_UI_RIGHT_START_W +
+                              box_pos_left),
+        static_cast<uint16_t>(Device::Referee::UIGetHeight() *
+                                  REF_UI_MODE_LINE2_H +
+                              REF_UI_BOX_UP_OFFSET),
+        static_cast<uint16_t>(Device::Referee::UIGetWidth() *
+                                  REF_UI_RIGHT_START_W +
+                              box_pos_right),
+        static_cast<uint16_t>(Device::Referee::UIGetHeight() *
+                                  REF_UI_MODE_LINE2_H +
+                              REF_UI_BOX_BOT_OFFSET));
+    Device::Referee::AddUI(gimbal->rectangle_);
+  }
+  gimbal->line_.Draw(
+      "g", Component::UI::UI_GRAPHIC_OP_ADD,
+      Component::UI::UI_GRAPHIC_LAYER_CONST, Component::UI::UI_GREEN,
+      UI_DEFAULT_WIDTH * 3,
+      static_cast<uint16_t>(Device::Referee::UIGetWidth() * 0.4f),
+      static_cast<uint16_t>(Device::Referee::UIGetHeight() * 0.2f),
+      static_cast<uint16_t>(Device::Referee::UIGetWidth() * 0.4f),
+      static_cast<uint16_t>(Device::Referee::UIGetHeight() * 0.2f + 50.f));
+  Device::Referee::AddUI(gimbal->line_);
+  gimbal->line_.Draw(
+      "GA", Component::UI::UI_GRAPHIC_OP_ADD,
+      Component::UI::UI_GRAPHIC_LAYER_GIMBAL, Component::UI::UI_GREEN,
+      UI_DEFAULT_WIDTH * 12,
+      static_cast<uint16_t>(Device::Referee::UIGetWidth() * 0.4f),
+      static_cast<uint16_t>(Device::Referee::UIGetHeight() * 0.2f),
+      static_cast<uint16_t>(Device::Referee::UIGetWidth() * 0.4f +
+                            -sinf(gimbal->gimbal_data_.yaw) * 44),
+      static_cast<uint16_t>(Device::Referee::UIGetHeight() * 0.2f +
+                            cosf(gimbal->gimbal_data_.yaw) * 44));
+  Device::Referee::AddUI(gimbal->line_);
+}
+
+void Gimbal::DrawUIDynamic(Gimbal* gimbal) {
+  float box_pos_left = 0.0f, box_pos_right = 0.0f;
+
+  /* 更新云台模式选择框 */
+  switch (gimbal->mode_) {
+    case RELAX:
+      box_pos_left = REF_UI_MODE_OFFSET_2_LEFT;
+      box_pos_right = REF_UI_MODE_OFFSET_2_RIGHT;
+      break;
+    case ABSOLUTE:
+      if (gimbal->cmd_.mode == Component::CMD::GIMBAL_ABSOLUTE_CTRL) {
+        box_pos_left = REF_UI_MODE_OFFSET_3_LEFT;
+        box_pos_right = REF_UI_MODE_OFFSET_3_RIGHT;
+      } else {
+        box_pos_left = REF_UI_MODE_OFFSET_4_LEFT;
+        box_pos_right = REF_UI_MODE_OFFSET_4_RIGHT;
+      }
+      break;
+    default:
+      box_pos_left = 0.0f;
+      box_pos_right = 0.0f;
+      break;
+  }
+  if (box_pos_left != 0.0f && box_pos_right != 0.0f) {
+    gimbal->rectangle_.Draw(
+        "GS", Component::UI::UI_GRAPHIC_OP_REWRITE,
+        Component::UI::UI_GRAPHIC_LAYER_GIMBAL, Component::UI::UI_GREEN,
+        UI_DEFAULT_WIDTH,
+        static_cast<uint16_t>(Device::Referee::UIGetWidth() *
+                                  REF_UI_RIGHT_START_W +
+                              box_pos_left),
+        static_cast<uint16_t>(Device::Referee::UIGetHeight() *
+                                  REF_UI_MODE_LINE2_H +
+                              REF_UI_BOX_UP_OFFSET),
+        static_cast<uint16_t>(Device::Referee::UIGetWidth() *
+                                  REF_UI_RIGHT_START_W +
+                              box_pos_right),
+        static_cast<uint16_t>(Device::Referee::UIGetHeight() *
+                                  REF_UI_MODE_LINE2_H +
+                              REF_UI_BOX_BOT_OFFSET));
+    Device::Referee::AddUI(gimbal->rectangle_);
+  }
+  gimbal->line_.Draw(
+      "GA", Component::UI::UI_GRAPHIC_OP_REWRITE,
+      Component::UI::UI_GRAPHIC_LAYER_GIMBAL, Component::UI::UI_GREEN,
+      UI_DEFAULT_WIDTH * 12,
+      static_cast<uint16_t>(Device::Referee::UIGetWidth() * 0.4f),
+      static_cast<uint16_t>(Device::Referee::UIGetHeight() * 0.2f),
+      static_cast<uint16_t>(Device::Referee::UIGetWidth() * 0.4f +
+                            -sinf(gimbal->gimbal_data_.yaw) * 44),
+      static_cast<uint16_t>(Device::Referee::UIGetHeight() * 0.2f +
+                            cosf(gimbal->gimbal_data_.yaw) * 44));
+  Device::Referee::AddUI(gimbal->line_);
+}
diff --git a/src/module/mit_gimbal/mod_mit_gimbal.hpp b/src/module/mit_gimbal/mod_mit_gimbal.hpp
new file mode 100644
index 00000000..9d1d6d92
--- /dev/null
+++ b/src/module/mit_gimbal/mod_mit_gimbal.hpp
@@ -0,0 +1,136 @@
+#pragma once
+
+#include <comp_type.hpp>
+#include <comp_ui.hpp>
+#include <module.hpp>
+
+#include "comp_actuator.hpp"
+#include "comp_cf.hpp"
+#include "comp_cmd.hpp"
+#include "comp_filter.hpp"
+#include "comp_pid.hpp"
+#include "dev_ahrs.hpp"
+#include "dev_bmi088.hpp"
+#include "dev_referee.hpp"
+#include "dev_rm_motor.hpp"
+
+namespace Module {
+class Gimbal {
+ public:
+  /* 云台运行模式 */
+  typedef enum {
+    RELAX, /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */
+    ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */
+  } Mode;
+
+  enum {
+    GIMBAL_CTRL_YAW_OMEGA_IDX = 0, /* Yaw轴控制的角速度环控制器的索引值 */
+    GIMBAL_CTRL_YAW_ANGLE_IDX, /* Yaw轴控制的角度环控制器的索引值 */
+    GIMBAL_CTRL_PIT_OMEGA_IDX, /* Pitch轴控制的角速度环控制器的索引值 */
+    GIMBAL_CTRL_PIT_ANGLE_IDX, /* Pitch轴控制的角度环控制器的索引值 */
+    GIMBAL_CTRL_NUM,           /* 总共的控制器数量 */
+  };
+
+  typedef enum {
+    SET_MODE_RELAX,
+    SET_MODE_ABSOLUTE,
+    START_AUTO_AIM,
+    STOP_AUTO_AIM,
+  } GimbalEvent;
+  typedef struct {
+    float yaw;
+    Component::Type::Eulr mech_zero;
+  } GimbalData;
+
+  typedef struct {
+    Component::SecOrderFunction::Param ff; /* PITCH前馈 */
+    Component::SecOrderFunction::Param st; /* YAW自整定参数 */
+
+    Component::PosActuator::Param yaw_actr;
+    Component::PosActuator::Param pit_actr;
+
+    Device::RMMotor::Param yaw_motor;
+    Device::RMMotor::Param pit_motor;
+
+    Component::Type::Eulr mech_zero;
+
+    float scanf_yaw_rate;
+    float scanf_pit_center;
+    float scanf_pit_range;
+    float scanf_pit_omega;
+
+    struct {
+      Component::Type::CycleValue pitch_max;
+      Component::Type::CycleValue pitch_min;
+      Component::Type::CycleValue yaw_max;
+      Component::Type::CycleValue yaw_min;
+    } limit;
+
+    const std::vector<Component::CMD::EventMapItem> EVENT_MAP;
+
+  } Param;
+  typedef struct {
+    Device::Referee::Status status;
+    float armor_id;
+  } RefForGImbal;
+
+  Gimbal(Param &param, float control_freq);
+
+  void UpdateFeedback();
+
+  void Control();
+
+  void SetMode(Mode mode);
+
+  void PraseRef();
+
+  static void DrawUIStatic(Gimbal *gimbal);
+
+  static void DrawUIDynamic(Gimbal *gimbal);
+
+ private:
+  uint64_t last_wakeup_ = 0;
+
+  uint64_t now_ = 0;
+
+  float dt_ = 0.0f;
+
+  Param param_;
+
+  Gimbal::Mode mode_ = RELAX; /* 云台模式 */
+
+  struct {
+    Component::Type::Eulr eulr_; /* 表示云台姿态的欧拉角 */
+  } setpoint_;
+
+  Component::SecOrderFunction st_; /* YAW自整定参数 */
+
+  Component::PosActuator yaw_actuator_;
+  Component::PosActuator pit_actuator_;
+
+  Device::RMMotor yaw_motor_;
+  Device::RMMotor pit_motor_;
+
+  Device::Referee::Data raw_ref_;
+  RefForGImbal ref_;
+
+  System::Thread thread_;
+
+  System::Semaphore ctrl_lock_;
+
+  Message::Topic<GimbalData> yaw_tp_ =
+      Message::Topic<GimbalData>("gimbal_data");
+
+  GimbalData gimbal_data_;
+
+  Component::UI::String string_;
+
+  Component::UI::Rectangle rectangle_;
+
+  Component::UI::Line line_;
+
+  Component::Type::Eulr eulr_;
+  Component::Type::Vector3 gyro_;
+  Component::CMD::GimbalCMD cmd_;
+};
+}  // namespace Module
diff --git a/src/robot/omni_infantry/Kconfig b/src/robot/omni_infantry/Kconfig
new file mode 100644
index 00000000..e69de29b
diff --git a/src/robot/omni_infantry/robot.cpp b/src/robot/omni_infantry/robot.cpp
new file mode 100644
index 00000000..f531df7d
--- /dev/null
+++ b/src/robot/omni_infantry/robot.cpp
@@ -0,0 +1,437 @@
+#include "robot.hpp"
+
+#include <comp_actuator.hpp>
+
+#include "dev_rm_motor.hpp"
+#include "mod_chassis.hpp"
+#include "system.hpp"
+
+/* clang-format off */
+Robot::Infantry::Param param = {
+    .chassis={
+      .type = Component::Mixer::MECANUM,
+
+      .follow_pid_param = {
+      .k = 0.5f,
+      .p = 1.0f,
+      .i = 0.0f,
+      .d = 0.0f,
+      .i_limit = 1.0f,
+      .out_limit = 1.0f,
+      .d_cutoff_freq = -1.0f,
+      .cycle = true,
+    },
+
+    .EVENT_MAP = {
+      Component::CMD::EventMapItem{
+        Component::CMD::CMD_EVENT_LOST_CTRL,
+        Module::RMChassis::SET_MODE_RELAX
+      },
+      Component::CMD::EventMapItem{
+        Device::DR16::DR16_SW_L_POS_TOP,
+        Module::RMChassis::SET_MODE_RELAX
+      },
+      Component::CMD::EventMapItem{
+        Device::DR16::DR16_SW_L_POS_MID,
+        Module::RMChassis::SET_MODE_FOLLOW
+      },
+      Component::CMD::EventMapItem{
+        Device::DR16::DR16_SW_L_POS_BOT,
+        Module::RMChassis::SET_MODE_ROTOR
+      },
+      Component::CMD::EventMapItem{
+        Device::DR16::KEY_V,
+        Module::RMChassis::SET_MODE_ROTOR
+      },
+      Component::CMD::EventMapItem{
+        Device::DR16::KEY_B,
+        Module::RMChassis::SET_MODE_FOLLOW
+      }
+    },
+
+    .actuator_param = {
+      Component::SpeedActuator::Param{
+        .speed = {
+          .k = 0.00015f,
+          .p = 1.0f,
+          .i = 0.0f,
+          .d = 0.0f,
+          .i_limit = 1.0f,
+          .out_limit = 1.0f,
+          .d_cutoff_freq = -1.0f,
+          .cycle = false,
+        },
+
+        .in_cutoff_freq = -1.0f,
+
+        .out_cutoff_freq = -1.0f,
+
+      },
+      Component::SpeedActuator::Param{
+        .speed = {
+          .k = 0.00018f,
+          .p = 1.0f,
+          .i = 0.0f,
+          .d = 0.0f,
+          .i_limit = 1.0f,
+          .out_limit = 1.0f,
+          .d_cutoff_freq = -1.0f,
+          .cycle = false,
+        },
+
+        .in_cutoff_freq = -1.0f,
+
+        .out_cutoff_freq = -1.0f,
+      },
+      Component::SpeedActuator::Param{
+        .speed = {
+          .k = 0.00015f,
+          .p = 1.0f,
+          .i = 0.0f,
+          .d = 0.0f,
+          .i_limit = 1.0f,
+          .out_limit = 1.0f,
+          .d_cutoff_freq = -1.0f,
+          .cycle = false,
+        },
+
+        .in_cutoff_freq = -1.0f,
+
+        .out_cutoff_freq = -1.0f,
+      },
+      Component::SpeedActuator::Param{
+        .speed = {
+          .k = 0.00015f,
+          .p = 1.0f,
+          .i = 0.0f,
+          .d = 0.0f,
+          .i_limit = 1.0f,
+          .out_limit = 1.0f,
+          .d_cutoff_freq = -1.0f,
+          .cycle = false,
+        },
+
+        .in_cutoff_freq = -1.0f,
+
+        .out_cutoff_freq = -1.0f,
+      },
+    },
+
+    .motor_param = {
+      Device::RMMotor::Param{
+          .id_feedback = 0x201,
+          .id_control = M3508_M2006_CTRL_ID_BASE,
+          .model = Device::RMMotor::MOTOR_M3508,
+          .can = BSP_CAN_1,
+      },
+      Device::RMMotor::Param{
+          .id_feedback = 0x202,
+          .id_control = M3508_M2006_CTRL_ID_BASE,
+          .model = Device::RMMotor::MOTOR_M3508,
+          .can = BSP_CAN_1,
+      },
+      Device::RMMotor::Param{
+          .id_feedback = 0x203,
+          .id_control = M3508_M2006_CTRL_ID_BASE,
+          .model = Device::RMMotor::MOTOR_M3508,
+          .can = BSP_CAN_1,
+      },
+      Device::RMMotor::Param{
+          .id_feedback = 0x204,
+          .id_control = M3508_M2006_CTRL_ID_BASE,
+          .model = Device::RMMotor::MOTOR_M3508,
+          .can = BSP_CAN_1,
+      },
+    },
+  },
+
+  .gimbal = {
+    .ff = {
+      /* GIMBAL_CTRL_PIT_FEEDFORWARD */
+      .a = 0.0439f,
+      .b = -0.0896f,
+      .c = 0.077f,
+      .max = 0.1f,
+      .min = -0.2f,
+    }, /* ff */
+
+    .st = {
+      /* GIMBAL_CTRL_YAW_SELF_TUNING */
+      .a = 0.0677f,
+      .b = 0.1653f,
+      .c = 0.3379f,
+      .max = 0.37f,
+      .min = 0.29f,
+    }, /* st */
+
+    .yaw_actr = {
+      .speed = {
+          /* GIMBAL_CTRL_YAW_OMEGA_IDX */
+          .k = 0.28f,
+          .p = 1.f,
+          .i = 1.f,
+          .d = 0.f,
+          .i_limit = 0.2f,
+          .out_limit = 1.0f,
+          .d_cutoff_freq = -1.0f,
+          .cycle = false,
+        },
+
+        .position = {
+          /* GIMBAL_CTRL_YAW_ANGLE_IDX */
+          .k = 20.0f,
+          .p = 1.0f,
+          .i = 0.0f,
+          .d = 0.0f,
+          .i_limit = 0.0f,
+          .out_limit = 10.0f,
+          .d_cutoff_freq = -1.0f,
+          .cycle = true,
+        },
+
+        .in_cutoff_freq = -1.0f,
+
+        .out_cutoff_freq = -1.0f,
+    },
+    .pit_actr = {
+        .speed = {
+          /* GIMBAL_CTRL_PIT_OMEGA_IDX */
+          .k = 0.25f,
+          .p = 1.0f,
+          .i = 0.f,
+          .d = 0.f,
+          .i_limit = 0.8f,
+          .out_limit = 1.0f,
+          .d_cutoff_freq = -1.0f,
+          .cycle = false,
+        },
+
+        .position = {
+          /* GIMBAL_CTRL_PIT_ANGLE_IDX */
+          .k = 20.0f,
+          .p = 1.0f,
+          .i = 0.0f,
+          .d = 0.0f,
+          .i_limit = 0.0f,
+          .out_limit = 10.0f,
+          .d_cutoff_freq = -1.0f,
+          .cycle = true,
+        },
+
+        .in_cutoff_freq = -1.0f,
+
+        .out_cutoff_freq = -1.0f,
+    },
+
+    .yaw_motor = {
+      .id_feedback = 0x209,
+      .id_control = GM6020_CTRL_ID_EXTAND,
+      .model = Device::RMMotor::MOTOR_GM6020,
+      .can = BSP_CAN_1,
+    },
+
+    .pit_motor = {
+      .id_feedback = 0x20A,
+      .id_control = GM6020_CTRL_ID_EXTAND,
+      .model = Device::RMMotor::MOTOR_GM6020,
+      .can = BSP_CAN_2,
+    },
+
+    .mech_zero = {
+      .yaw = 1.3f,
+      .pit = 4.0f,
+      .rol = 0.0f,
+    },
+
+    .limit = {
+      .pitch_max = 3.8f,
+      .pitch_min = 3.0f,
+    },
+
+    .EVENT_MAP = {
+      Component::CMD::EventMapItem{
+        Component::CMD::CMD_EVENT_LOST_CTRL,
+        Module::Gimbal::SET_MODE_RELAX
+      },
+      Component::CMD::EventMapItem{
+        Device::DR16::DR16_SW_R_POS_TOP,
+        Module::Gimbal::SET_MODE_ABSOLUTE
+      },
+      Component::CMD::EventMapItem{
+        Device::DR16::DR16_SW_R_POS_MID,
+        Module::Gimbal::SET_MODE_ABSOLUTE
+      },
+      Component::CMD::EventMapItem{
+        Device::DR16::DR16_SW_R_POS_BOT,
+        Module::Gimbal::SET_MODE_ABSOLUTE
+      },
+      Component::CMD::EventMapItem{
+        Device::DR16::KEY_R_PRESS,
+        Module::Gimbal::START_AUTO_AIM
+      },
+      Component::CMD::EventMapItem{
+        Device::DR16::KEY_R_RELEASE,
+        Module::Gimbal::STOP_AUTO_AIM
+      }
+    },
+
+  },
+
+  .launcher = {
+    .num_trig_tooth = 8.0f,
+    .trig_gear_ratio = 36.0f,
+    .fric_radius = 0.03f,
+    .cover_open_duty = 0.125f,
+    .cover_close_duty = 0.075f,
+    .model = Module::Launcher::LAUNCHER_MODEL_17MM,
+    .default_bullet_speed = 15.f,
+    .min_launch_delay = static_cast<uint32_t>(1000.0f / 20.0f),
+
+    .trig_actr = {
+      Component::PosActuator::Param{
+        .speed = {
+          .k = 1.5f,
+          .p = 1.0f,
+          .i = 0.0f,
+          .d = 0.03f,
+          .i_limit = 0.5f,
+          .out_limit = 0.5f,
+          .d_cutoff_freq = -1.0f,
+          .cycle = false,
+        },
+
+        .position = {
+          .k = 1.2f,
+          .p = 1.0f,
+          .i = 0.0f,
+          .d = 0.012f,
+          .i_limit = 1.0f,
+          .out_limit = 1.0f,
+          .d_cutoff_freq = -1.0f,
+          .cycle = true,
+        },
+
+        .in_cutoff_freq = -1.0f,
+
+        .out_cutoff_freq = -1.0f,
+      },
+    },
+
+    .fric_actr = {
+      Component::SpeedActuator::Param{
+        .speed = {
+          .k = 0.00025f,
+          .p = 1.0f,
+          .i = 0.4f,
+          .d = 0.01f,
+          .i_limit = 0.5f,
+          .out_limit = 1.0f,
+          .d_cutoff_freq = -1.0f,
+          .cycle = false,
+        },
+
+        .in_cutoff_freq = -1.0f,
+
+        .out_cutoff_freq = -1.0f,
+      },
+      Component::SpeedActuator::Param{
+        .speed = {
+          .k = 0.00025f,
+          .p = 1.0f,
+          .i = 0.4f,
+          .d = 0.01f,
+          .i_limit = 0.5f,
+          .out_limit = 1.0f,
+          .d_cutoff_freq = -1.0f,
+          .cycle = false,
+        },
+
+        .in_cutoff_freq = -1.0f,
+
+        .out_cutoff_freq = -1.0f,
+      },
+    },
+
+    .trig_motor = {
+      Device::RMMotor::Param{
+        .id_feedback = 0x207,
+        .id_control = M3508_M2006_CTRL_ID_EXTAND,
+        .model = Device::RMMotor::MOTOR_M2006,
+        .can = BSP_CAN_2,
+      }
+    },
+
+    .fric_motor = {
+      Device::RMMotor::Param{
+          .id_feedback = 0x205,
+          .id_control = M3508_M2006_CTRL_ID_EXTAND,
+          .model = Device::RMMotor::MOTOR_M3508,
+          .can = BSP_CAN_2,
+      },
+      Device::RMMotor::Param{
+          .id_feedback = 0x206,
+          .id_control = M3508_M2006_CTRL_ID_EXTAND,
+          .model = Device::RMMotor::MOTOR_M3508,
+          .can = BSP_CAN_2,
+      },
+    },
+
+    .EVENT_MAP = {
+      Component::CMD::EventMapItem{
+        Component::CMD::CMD_EVENT_LOST_CTRL,
+        Module::Launcher::CHANGE_FIRE_MODE_RELAX
+      },
+      Component::CMD::EventMapItem{
+        Device::DR16::DR16_SW_R_POS_TOP,
+        Module::Launcher::CHANGE_FIRE_MODE_SAFE
+      },
+      Component::CMD::EventMapItem{
+        Device::DR16::DR16_SW_R_POS_MID,
+        Module::Launcher::CHANGE_FIRE_MODE_LOADED
+      },
+      Component::CMD::EventMapItem{
+        Device::DR16::DR16_SW_R_POS_BOT,
+        Module::Launcher::CHANGE_FIRE_MODE_LOADED
+      },
+      Component::CMD::EventMapItem{
+        Device::DR16::DR16_SW_R_POS_BOT,
+        Module::Launcher::LAUNCHER_START_FIRE
+      },
+      Component::CMD::EventMapItem{
+        Device::DR16::KEY_L_PRESS,
+        Module::Launcher::LAUNCHER_START_FIRE
+      },
+      Component::CMD::EventMapItem{
+        Device::DR16::KEY_G,
+        Module::Launcher::CHANGE_TRIG_MODE
+      },
+      Component::CMD::EventMapItem{
+        Device::DR16::KEY_R,
+        Module::Launcher::OPEN_COVER
+      },
+      Component::CMD::EventMapItem{
+        Device::DR16::KEY_F,
+        Module::Launcher::CLOSE_COVER
+      }
+    },
+  }, /* launcher */
+
+  .bmi088_rot = {
+    .rot_mat = {
+      { +1, +0, +0},
+      { +0, +1, +0},
+      { +0, +0, +1},
+    },
+  },
+
+  .cap = {
+    .can = BSP_CAN_1,
+    .index = DEV_CAP_FB_ID_BASE,
+    .cutoff_volt = 13.0f,
+  },
+};
+/* clang-format on */
+
+void robot_init() {
+  System::Start<Robot::Infantry, Robot::Infantry::Param>(param, 500.0f);
+}
diff --git a/src/robot/omni_infantry/robot.hpp b/src/robot/omni_infantry/robot.hpp
new file mode 100644
index 00000000..2a0f8a40
--- /dev/null
+++ b/src/robot/omni_infantry/robot.hpp
@@ -0,0 +1,48 @@
+#include "comp_cmd.hpp"
+#include "dev_ahrs.hpp"
+#include "dev_ai.hpp"
+#include "dev_bmi088.hpp"
+#include "dev_can.hpp"
+#include "dev_cap.hpp"
+#include "dev_dr16.hpp"
+#include "dev_led_rgb.hpp"
+#include "dev_referee.hpp"
+#include "mod_chassis.hpp"
+#include "mod_launcher.hpp"
+#include "mod_mit_gimbal.hpp"
+
+void robot_init();
+namespace Robot {
+class Infantry {
+ public:
+  typedef struct Param {
+    Module::RMChassis::Param chassis;
+    Module::Gimbal::Param gimbal;
+    Module::Launcher::Param launcher;
+    Device::BMI088::Rotation bmi088_rot{};
+    Device::Cap::Param cap{};
+  } Param;
+
+  Component::CMD cmd_;
+
+  Device::AI ai_;
+  Device::AHRS ahrs_;
+  Device::BMI088 bmi088_;
+  Device::Can can_;
+  Device::Cap cap_;
+  Device::DR16 dr16_;
+  Device::RGB led_;
+  Device::Referee referee_;
+
+  Module::RMChassis chassis_;
+  Module::Gimbal gimbal_;
+  Module::Launcher launcher_;
+
+  Infantry(Param& param, float control_freq)
+      : bmi088_(param.bmi088_rot),
+        cap_(param.cap),
+        chassis_(param.chassis, control_freq),
+        gimbal_(param.gimbal, control_freq),
+        launcher_(param.launcher, control_freq) {}
+};
+}  // namespace Robot
-- 
Gitee


From 47d0f346f6171b0d61c36bfe08c44d58a16c392f Mon Sep 17 00:00:00 2001
From: Marine_zxy <1423266892@qq.com>
Date: Wed, 8 May 2024 20:45:30 +0800
Subject: [PATCH 08/19] =?UTF-8?q?=E6=9B=B4=E6=96=B0MIT=E7=94=B5=E6=9C=BApi?=
 =?UTF-8?q?tch=E8=BD=B4?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/module/mit_gimbal/mod_mit_gimbal.hpp | 5 +++--
 src/robot/omni_infantry/robot.cpp        | 9 ++++++---
 2 files changed, 9 insertions(+), 5 deletions(-)

diff --git a/src/module/mit_gimbal/mod_mit_gimbal.hpp b/src/module/mit_gimbal/mod_mit_gimbal.hpp
index 9d1d6d92..f1a0e375 100644
--- a/src/module/mit_gimbal/mod_mit_gimbal.hpp
+++ b/src/module/mit_gimbal/mod_mit_gimbal.hpp
@@ -11,6 +11,7 @@
 #include "comp_pid.hpp"
 #include "dev_ahrs.hpp"
 #include "dev_bmi088.hpp"
+#include "dev_mit_motor.hpp"
 #include "dev_referee.hpp"
 #include "dev_rm_motor.hpp"
 
@@ -50,7 +51,7 @@ class Gimbal {
     Component::PosActuator::Param pit_actr;
 
     Device::RMMotor::Param yaw_motor;
-    Device::RMMotor::Param pit_motor;
+    Device::MitMotor::Param pit_motor;
 
     Component::Type::Eulr mech_zero;
 
@@ -109,7 +110,7 @@ class Gimbal {
   Component::PosActuator pit_actuator_;
 
   Device::RMMotor yaw_motor_;
-  Device::RMMotor pit_motor_;
+  Device::MitMotor pit_motor_;
 
   Device::Referee::Data raw_ref_;
   RefForGImbal ref_;
diff --git a/src/robot/omni_infantry/robot.cpp b/src/robot/omni_infantry/robot.cpp
index f531df7d..b3246de7 100644
--- a/src/robot/omni_infantry/robot.cpp
+++ b/src/robot/omni_infantry/robot.cpp
@@ -231,10 +231,13 @@ Robot::Infantry::Param param = {
     },
 
     .pit_motor = {
-      .id_feedback = 0x20A,
-      .id_control = GM6020_CTRL_ID_EXTAND,
-      .model = Device::RMMotor::MOTOR_GM6020,
+      .kp = 0.0f,
+      .kd = 0.0f,
+      .def_speed = 0.0,
+      .id = 0x0,
       .can = BSP_CAN_2,
+      .max_error = 0.0,
+      .reverse = false,
     },
 
     .mech_zero = {
-- 
Gitee


From c00734e1494621dc09e831500dbaff0bd86ab5e2 Mon Sep 17 00:00:00 2001
From: Marine_zxy <1423266892@qq.com>
Date: Wed, 8 May 2024 23:07:39 +0800
Subject: [PATCH 09/19] =?UTF-8?q?=E5=AE=8C=E5=96=84=E5=93=A8=E5=85=B5?=
 =?UTF-8?q?=E4=BA=91=E5=8F=B0=E6=89=AB=E6=8F=8F=E6=A8=A1=E5=BC=8F?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/device/ai/dev_ai.cpp         | 138 +++++++++++++++++--------------
 src/device/ai/dev_ai.hpp         |  34 ++++----
 src/module/gimbal/mod_gimbal.cpp |   1 +
 src/module/gimbal/mod_gimbal.hpp |  13 ++-
 src/robot/sentry/robot.cpp       |  45 +++++-----
 src/robot/sentry/robot.hpp       |   2 +-
 6 files changed, 128 insertions(+), 105 deletions(-)

diff --git a/src/device/ai/dev_ai.cpp b/src/device/ai/dev_ai.cpp
index 2e63cf1a..837f093a 100644
--- a/src/device/ai/dev_ai.cpp
+++ b/src/device/ai/dev_ai.cpp
@@ -139,12 +139,17 @@ bool AI::PackRef() {
 
   return true;
 }
-void AI::GetGimbalData() {}
+
 /* 后续通信协议要改：
  1、电控say导航到哪里
  2、视觉say是否是导航状态（电控要求一个位置，如果导航到或者已经在此为止，则算法反馈一个“已完成”，表示到达目的地附近）
  3、建议notice分开成
  底盘、云台、发射三个notice，方便三路分开也避免错误置零的情况
+
+底盘：电控->视觉123
+云台：视觉->电控 是否看见
+发射：视觉->电控 能否发弹
+
  */
 void AI::DecideAction() {
   this->notice_ = this->from_host_.data.notice;
@@ -152,6 +157,7 @@ void AI::DecideAction() {
   /* 判定是否受击打 */
   if (this->raw_ref_.robot_damage.damage_type == 0x0 && !last_damage_type_) {
     this->is_damaged_ = true;
+    this->damaged_id_ = this->raw_ref_.robot_damage.armor_id;
   }
   this->last_damage_type_ = this->raw_ref_.robot_damage.damage_type;
 
@@ -168,9 +174,9 @@ void AI::DecideAction() {
       this->action_.ai_chassis = AI::Action::TO_OUTPOST;
     } else {
       if (this->ref_.hp < 50) { /* max = 400 */
-        this->action_.ai_chassis = AI::Action::TO_PATROL_AREA;
-      } else {
         this->action_.ai_chassis = AI::Action::TO_SUPPLY;
+      } else {
+        this->action_.ai_chassis = AI::Action::TO_PATROL_AREA;
       }
 
       //   this->action_.ai_chassis = AI::Action::TO_HIGHLAND;
@@ -187,6 +193,7 @@ void AI::DecideAction() {
   } else if (is_damaged_) { /* 反击 */
     this->cmd_.gimbal.mode = Component::CMD::GIMBAL_ABSOLUTE_CTRL;
     this->action_.ai_gimbal = AI::Action::AFFECTED;
+    is_damaged_ = false;
   } else { /* 扫描 */
     this->cmd_.gimbal.mode = Component::CMD::GIMBAL_ABSOLUTE_CTRL;
     this->action_.ai_gimbal = AI::Action::SCANF;
@@ -223,71 +230,74 @@ bool AI::PackCMD() {
 
   /* 控制权在AI */
   if (Component::CMD::GetCtrlSource() == Component::CMD::CTRL_SOURCE_AI) {
-    if (this->cmd_.online) { /* AI在线 */
-      switch (this->action_.ai_chassis) {
-        case TO_BASE:
-        case TO_OUTPOST:
-        case TO_SUPPLY:
-        case TO_HIGHLAND:
-        case TO_PATROL_AREA:
-          memcpy(&(this->cmd_.chassis),
-                 &(this->from_host_.data.chassis_move_vec),
-                 sizeof(this->from_host_.data.chassis_move_vec));
-          break;
-        case ROTOR:
-          this->cmd_.chassis.x = 0;
-          this->cmd_.chassis.y = 0;
-          this->cmd_.chassis.z = 0.5;  //待修改
-          break;
-      }
+    // if (this->cmd_.online) { /* AI在线 */
+    switch (this->action_.ai_chassis) {
+      case TO_BASE:
+      case TO_OUTPOST:
+      case TO_SUPPLY:
+      case TO_HIGHLAND:
+      case TO_PATROL_AREA:
+        memcpy(&(this->cmd_.chassis), &(this->from_host_.data.chassis_move_vec),
+               sizeof(this->from_host_.data.chassis_move_vec));
+        break;
+      case ROTOR:
+        this->cmd_.chassis.x = 0;
+        this->cmd_.chassis.y = 0;
+        this->cmd_.chassis.z = 0.5;  //待修改
+        break;
+    }
 
-      switch (this->action_.ai_gimbal) {
-        case AUTO_AIM:
-          memcpy(&(this->cmd_.gimbal.eulr), &(this->from_host_.data.gimbal),
-                 sizeof(this->cmd_.gimbal.eulr));
-          break;
-        case AFFECTED:
-          this->cmd_.gimbal.eulr.yaw =
-              this->gimbal_data_.mech_zero.yaw +
-              this->ref_.damaged_armor_id * (M_2PI / 4.0);
-          this->cmd_.gimbal.eulr.pit = 0;  //待修改成合适角度
-          break;
-        case SCANF:
-          this->cmd_.gimbal.eulr.yaw = 0;
-          this->cmd_.gimbal.eulr.pit = 0;
-          /* 待写入合适的扫描函数 */
-          break;
-      }
+    switch (this->action_.ai_gimbal) {
+      case AUTO_AIM:
+        memcpy(&(this->cmd_.gimbal.eulr), &(this->from_host_.data.gimbal),
+               sizeof(this->cmd_.gimbal.eulr));
+        break;
+      case AFFECTED:
+        this->cmd_.gimbal.eulr.yaw = this->damaged_id_ * (M_2PI / 4.0);
+        this->test_ = this->cmd_.gimbal.eulr.yaw;
+        this->cmd_.gimbal.eulr.pit = 4.5f;  //待修改成合适角度
+        break;
+      case SCANF:
+        this->cmd_.gimbal.eulr.yaw =
+            this->gimbal_data_.scanf_mode.scanf_yaw_rate;
+        this->cmd_.gimbal.eulr.pit =
+            this->gimbal_data_.scanf_mode.scanf_pit_center +
+            this->gimbal_data_.scanf_mode.scanf_pit_range *
+                sin(this->gimbal_data_.scanf_mode.scanf_pit_omega *
+                    bsp_time_get_ms());
+        /* 待写入合适的扫描函数 */
+        break;
+    }
 
-      switch (this->action_.ai_launcher) {
-        case FIRE:
-          this->event_.Active(AI_FIRE_COMMAND);  //写热量控制！！最大化利用
-        case CEASEFIRE:
-          break;
-      }
+    switch (this->action_.ai_launcher) {
+      case FIRE:
+        this->event_.Active(AI_FIRE_COMMAND);  //写热量控制！！最大化利用
+      case CEASEFIRE:
+        break;
+    }
 
-      switch (this->action_.ai_to_referee) {  //需要和referee.cpp产生联系
-        case CONFIRM_RESURRECTION:
-          cmd_for_ref_.confirm_resurrection = 1;
-          cmd_for_ref_.buy_resurrection = 0;
-          cmd_for_ref_.buy_bullet_num = this->last_buy_bullet_num_;
-          cmd_for_ref_.remote_buy_bullet_times = 0;
-          cmd_for_ref_.romote_buy_hp_times = 0;
-          break;
-        case EXCHANGE_BULLETS:
-          cmd_for_ref_.confirm_resurrection = 0;
-          cmd_for_ref_.buy_resurrection = 0;
-          cmd_for_ref_.buy_bullet_num = this->last_buy_bullet_num_ + 50;
-          cmd_for_ref_.remote_buy_bullet_times = 0;
-          cmd_for_ref_.romote_buy_hp_times = 0;
-          break;
-        case NOTHING:
-          break;
-      }
-      this->last_buy_bullet_num_ = cmd_for_ref_.buy_bullet_num;
-    } else {
-      this->event_.Active(AI_OFFLINE);  //这个逻辑是对的
+    switch (this->action_.ai_to_referee) {  //需要和referee.cpp产生联系
+      case CONFIRM_RESURRECTION:
+        cmd_for_ref_.confirm_resurrection = 1;
+        cmd_for_ref_.buy_resurrection = 0;
+        cmd_for_ref_.buy_bullet_num = this->last_buy_bullet_num_;
+        cmd_for_ref_.remote_buy_bullet_times = 0;
+        cmd_for_ref_.romote_buy_hp_times = 0;
+        break;
+      case EXCHANGE_BULLETS:
+        cmd_for_ref_.confirm_resurrection = 0;
+        cmd_for_ref_.buy_resurrection = 0;
+        cmd_for_ref_.buy_bullet_num = this->last_buy_bullet_num_ + 50;
+        cmd_for_ref_.remote_buy_bullet_times = 0;
+        cmd_for_ref_.romote_buy_hp_times = 0;
+        break;
+      case NOTHING:
+        break;
     }
+    this->last_buy_bullet_num_ = cmd_for_ref_.buy_bullet_num;
+    // } else {
+    // this->event_.Active(AI_OFFLINE);  //这个逻辑是对的
+    // }
   }
 
   // memcpy(&(this->last_eulr_), &(this->cmd_.gimbal.eulr),
diff --git a/src/device/ai/dev_ai.hpp b/src/device/ai/dev_ai.hpp
index bd601d23..a8d03f41 100644
--- a/src/device/ai/dev_ai.hpp
+++ b/src/device/ai/dev_ai.hpp
@@ -42,10 +42,16 @@ class AI {
     float target_pos_y;
     uint8_t damaged_armor_id;
   } RefForAI;
-
+  typedef struct {
+    float scanf_yaw_rate;
+    float scanf_pit_center;
+    float scanf_pit_range;
+    float scanf_pit_omega;
+  } ScanfMode;
   typedef struct {
     float yaw;
     Component::Type::Eulr mech_zero;
+    ScanfMode scanf_mode;
   } GimbalData;
 
   typedef enum {
@@ -56,21 +62,21 @@ class AI {
     AI_FIRE_COMMAND,
   } AIControlData;
   typedef enum {
-    TO_BASE,
-    TO_OUTPOST,
-    TO_SUPPLY,
-    TO_HIGHLAND,
+    TO_BASE,        // 0
+    TO_OUTPOST,     // 1
+    TO_SUPPLY,      // 2
+    TO_HIGHLAND,    // 3
     TO_PATROL_AREA, /* 基地前巡逻区 */
-    ROTOR,
+    ROTOR,          // 5
 
-    SCANF,
-    AUTO_AIM,
-    AFFECTED, /* 反击*/
+    SCANF,     // 6
+    AUTO_AIM,  // 7
+    AFFECTED,  /* 反击*/
 
     CEASEFIRE, /* 停火 */
-    FIRE,
+    FIRE,      // 10
 
-    NOTHING,
+    NOTHING,              // 11
     CONFIRM_RESURRECTION, /* 确认复活 */
     EXCHANGE_BULLETS,     /* 兑换弹丸 */
     /* 其他行为暂不考虑 */
@@ -90,6 +96,7 @@ class AI {
     uint8_t ai_launcher;   /* 不发弹丸、开火 */
     uint8_t ai_to_referee; /* 确认复活、购买发弹量、购买血量 */
   } AICtrlAction;
+
   AI();
 
   bool StartRecv();
@@ -108,8 +115,6 @@ class AI {
 
   bool PackCMD();
 
-  void GetGimbalData();
-
   void DecideAction();
 
  private:
@@ -131,6 +136,7 @@ class AI {
   RefForAI ref_{};
   uint8_t last_damage_type_;
   bool is_damaged_;
+  uint8_t damaged_id_;
 
   System::Thread thread_;
 
@@ -141,7 +147,7 @@ class AI {
   Message::Topic<Component::CMD::Data> cmd_tp_;
 
   Component::CMD::Data cmd_{};
-
+  float test_;
   AICtrlAction action_;
 
   Component::Type::Eulr mech_zero_;
diff --git a/src/module/gimbal/mod_gimbal.cpp b/src/module/gimbal/mod_gimbal.cpp
index bf3bd2f4..62ffa5cf 100644
--- a/src/module/gimbal/mod_gimbal.cpp
+++ b/src/module/gimbal/mod_gimbal.cpp
@@ -86,6 +86,7 @@ void Gimbal::UpdateFeedback() {
       break;
   }
   this->gimbal_data_.mech_zero = this->param_.mech_zero;
+  this->gimbal_data_.scanf_mode = this->param_.scanf_mode;
 }
 
 void Gimbal::Control() {
diff --git a/src/module/gimbal/mod_gimbal.hpp b/src/module/gimbal/mod_gimbal.hpp
index 9d1d6d92..e3d72380 100644
--- a/src/module/gimbal/mod_gimbal.hpp
+++ b/src/module/gimbal/mod_gimbal.hpp
@@ -37,9 +37,17 @@ class Gimbal {
     START_AUTO_AIM,
     STOP_AUTO_AIM,
   } GimbalEvent;
+
+  typedef struct {
+    float scanf_yaw_rate;
+    float scanf_pit_center;
+    float scanf_pit_range;
+    float scanf_pit_omega;
+  } ScanfMode;
   typedef struct {
     float yaw;
     Component::Type::Eulr mech_zero;
+    ScanfMode scanf_mode;
   } GimbalData;
 
   typedef struct {
@@ -54,10 +62,7 @@ class Gimbal {
 
     Component::Type::Eulr mech_zero;
 
-    float scanf_yaw_rate;
-    float scanf_pit_center;
-    float scanf_pit_range;
-    float scanf_pit_omega;
+    ScanfMode scanf_mode;
 
     struct {
       Component::Type::CycleValue pitch_max;
diff --git a/src/robot/sentry/robot.cpp b/src/robot/sentry/robot.cpp
index 1892d2a6..0719f003 100644
--- a/src/robot/sentry/robot.cpp
+++ b/src/robot/sentry/robot.cpp
@@ -163,11 +163,11 @@ Robot::Sentry::Param param = {
     .yaw_actr = {
       .speed = {
           /* GIMBAL_CTRL_YAW_OMEGA_IDX */
-          .k = 1.2f,
-          .p = 1.0f,
-          .i = 0.1f,
+          .k = 0.28f,
+          .p = 1.f,
+          .i = 1.f,
           .d = 0.f,
-          .i_limit = 0.6f,
+          .i_limit = 0.2f,
           .out_limit = 1.0f,
           .d_cutoff_freq = -1.0f,
           .cycle = false,
@@ -175,12 +175,12 @@ Robot::Sentry::Param param = {
 
         .position = {
           /* GIMBAL_CTRL_YAW_ANGLE_IDX */
-          .k = 8.0f,
+          .k = 20.0f,
           .p = 1.0f,
           .i = 0.0f,
           .d = 0.0f,
           .i_limit = 0.0f,
-          .out_limit = 20.0f,
+          .out_limit = 10.0f,
           .d_cutoff_freq = -1.0f,
           .cycle = true,
         },
@@ -189,12 +189,12 @@ Robot::Sentry::Param param = {
 
         .out_cutoff_freq = -1.0f,
     },
-    .pit_actr = {
+   .pit_actr = {
         .speed = {
           /* GIMBAL_CTRL_PIT_OMEGA_IDX */
-          .k = 0.3f,
+          .k = 0.1f,
           .p = 1.0f,
-          .i = 0.5f,
+          .i = 0.0f,
           .d = 0.f,
           .i_limit = 0.8f,
           .out_limit = 1.0f,
@@ -204,7 +204,7 @@ Robot::Sentry::Param param = {
 
         .position = {
           /* GIMBAL_CTRL_PIT_ANGLE_IDX */
-          .k = 10.0f,
+          .k = 33.0f,
           .p = 1.0f,
           .i = 0.0f,
           .d = 0.0f,
@@ -218,7 +218,6 @@ Robot::Sentry::Param param = {
 
         .out_cutoff_freq = -1.0f,
     },
-
     .yaw_motor = {
       .id_feedback = 0x206,
       .id_control = GM6020_CTRL_ID_BASE,
@@ -234,19 +233,20 @@ Robot::Sentry::Param param = {
     },
 
     .mech_zero = {
-      .yaw = 5.55f,
-      .pit = 0.50f,
+      .yaw = 1.58f,
+      .pit = 4.7f,
       .rol = 0.0f,
     },
-
-    .scanf_yaw_rate = 2.0f,
-    .scanf_pit_center = 0.6f,
-    .scanf_pit_range = 0.2f,/* 都待修改 */
-    .scanf_pit_omega = 6.0f,
+    .scanf_mode{
+      .scanf_yaw_rate = 0.0f,
+      .scanf_pit_center = 4.7f,
+      .scanf_pit_range = 0.3f,
+      .scanf_pit_omega = 2.0f,
+    },
 
     .limit = {
-      .pitch_max = 0.60f,
-      .pitch_min = 0.19f,
+      .pitch_max = 4.9f,
+      .pitch_min = 4.46f,
     },
 
     .EVENT_MAP = {
@@ -351,6 +351,7 @@ Robot::Sentry::Param param = {
         .id_control = M3508_M2006_CTRL_ID_EXTAND,
         .model = Device::RMMotor::MOTOR_M2006,
         .can = BSP_CAN_2,
+        .reverse = true,
       }
     },
 
@@ -492,14 +493,14 @@ Robot::Sentry::Param param = {
           .id_control = M3508_M2006_CTRL_ID_BASE,
           .model = Device::RMMotor::MOTOR_M3508,
           .can = BSP_CAN_2,
-          .reverse = true,
+          .reverse = false,
       },
       Device::RMMotor::Param{
           .id_feedback = 0x201,
           .id_control = M3508_M2006_CTRL_ID_BASE,
           .model = Device::RMMotor::MOTOR_M3508,
           .can = BSP_CAN_2,
-          .reverse = true,
+          .reverse = false,
       },
     },
 
diff --git a/src/robot/sentry/robot.hpp b/src/robot/sentry/robot.hpp
index 0694c301..29b5d8a6 100644
--- a/src/robot/sentry/robot.hpp
+++ b/src/robot/sentry/robot.hpp
@@ -40,7 +40,7 @@ class Sentry {
   Module::Launcher launcher1_;
   Module::Launcher launcher2_;
   Sentry(Param& param, float control_freq)
-      : cmd_(Component::CMD::CMD_OP_CTRL),
+      : cmd_(Component::CMD::CMD_AUTO_CTRL),
         bmi088_(param.bmi088_rot),
         cap_(param.cap),
         chassis_(param.chassis, control_freq),
-- 
Gitee


From 29d756b49c92778631efeef70950b7f86f63c2e8 Mon Sep 17 00:00:00 2001
From: Marine_zxy <1423266892@qq.com>
Date: Thu, 9 May 2024 15:34:58 +0800
Subject: [PATCH 10/19] =?UTF-8?q?=E5=93=A8=E5=85=B5=E4=BA=91=E5=8F=B0?=
 =?UTF-8?q?=E6=89=AB=E6=8F=8F=E5=AE=8C=E6=88=90?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/device/ai/dev_ai.cpp           |  9 ++++-----
 src/device/ai/dev_ai.hpp           |  6 +++---
 src/module/chassis/mod_chassis.cpp |  6 +++---
 src/module/chassis/mod_chassis.hpp |  2 +-
 src/module/gimbal/mod_gimbal.cpp   | 12 +++++-------
 src/module/gimbal/mod_gimbal.hpp   |  3 +--
 src/robot/sentry/robot.cpp         | 10 +++++-----
 7 files changed, 22 insertions(+), 26 deletions(-)

diff --git a/src/device/ai/dev_ai.cpp b/src/device/ai/dev_ai.cpp
index 837f093a..6361e53f 100644
--- a/src/device/ai/dev_ai.cpp
+++ b/src/device/ai/dev_ai.cpp
@@ -254,18 +254,17 @@ bool AI::PackCMD() {
         break;
       case AFFECTED:
         this->cmd_.gimbal.eulr.yaw = this->damaged_id_ * (M_2PI / 4.0);
-        this->test_ = this->cmd_.gimbal.eulr.yaw;
         this->cmd_.gimbal.eulr.pit = 4.5f;  //待修改成合适角度
         break;
       case SCANF:
-        this->cmd_.gimbal.eulr.yaw =
-            this->gimbal_data_.scanf_mode.scanf_yaw_rate;
+        this->delta_angle_ += this->gimbal_data_.scanf_mode.scanf_yaw_rate;
+        this->cmd_.gimbal.eulr.yaw = this->delta_angle_;
+
         this->cmd_.gimbal.eulr.pit =
             this->gimbal_data_.scanf_mode.scanf_pit_center +
             this->gimbal_data_.scanf_mode.scanf_pit_range *
                 sin(this->gimbal_data_.scanf_mode.scanf_pit_omega *
-                    bsp_time_get_ms());
-        /* 待写入合适的扫描函数 */
+                    bsp_time_get_ms() / 1000.0);
         break;
     }
 
diff --git a/src/device/ai/dev_ai.hpp b/src/device/ai/dev_ai.hpp
index a8d03f41..f25d4e97 100644
--- a/src/device/ai/dev_ai.hpp
+++ b/src/device/ai/dev_ai.hpp
@@ -49,8 +49,7 @@ class AI {
     float scanf_pit_omega;
   } ScanfMode;
   typedef struct {
-    float yaw;
-    Component::Type::Eulr mech_zero;
+    float yaw_for_chassis;
     ScanfMode scanf_mode;
   } GimbalData;
 
@@ -147,7 +146,7 @@ class AI {
   Message::Topic<Component::CMD::Data> cmd_tp_;
 
   Component::CMD::Data cmd_{};
-  float test_;
+
   AICtrlAction action_;
 
   Component::Type::Eulr mech_zero_;
@@ -156,6 +155,7 @@ class AI {
 
   SentryDecisionData cmd_for_ref_;
   uint8_t last_buy_bullet_num_ = 0;
+  Component::Type::CycleValue delta_angle_ = 0.0;
 
   Message::Topic<SentryDecisionData> ai_tp_ =
       Message::Topic<SentryDecisionData>("sentry_cmd_for_ref");
diff --git a/src/module/chassis/mod_chassis.cpp b/src/module/chassis/mod_chassis.cpp
index 1913e92a..d3c3f14d 100644
--- a/src/module/chassis/mod_chassis.cpp
+++ b/src/module/chassis/mod_chassis.cpp
@@ -163,7 +163,7 @@ void Chassis<Motor, MotorParam>::Control() {
     case Chassis::FOLLOW_GIMBAL: /* 按照云台方向换算运动向量
                                   */
     case Chassis::ROTOR: {
-      float beta = this->gimbal_data_.yaw;
+      float beta = this->gimbal_data_.yaw_for_chassis;
       float cos_beta = cosf(beta);
       float sin_beta = sinf(beta);
       this->move_vec_.vx = cos_beta * this->cmd_.x - sin_beta * this->cmd_.y;
@@ -184,8 +184,8 @@ void Chassis<Motor, MotorParam>::Control() {
 
     case Chassis::FOLLOW_GIMBAL: /* 跟随模式通过PID控制使车头跟随云台
                                   */
-      this->move_vec_.wz =
-          this->follow_pid_.Calculate(0.0f, this->gimbal_data_.yaw, this->dt_);
+      this->move_vec_.wz = this->follow_pid_.Calculate(
+          0.0f, this->gimbal_data_.yaw_for_chassis, this->dt_);
       break;
 
     case Chassis::ROTOR: { /* 小陀螺模式使底盘以一定速度旋转
diff --git a/src/module/chassis/mod_chassis.hpp b/src/module/chassis/mod_chassis.hpp
index b5aa6b5e..197759dd 100644
--- a/src/module/chassis/mod_chassis.hpp
+++ b/src/module/chassis/mod_chassis.hpp
@@ -44,7 +44,7 @@ class Chassis {
   } ChassisEvent;
 
   typedef struct {
-    float yaw;
+    float yaw_for_chassis;
     Component::Type::Eulr mech_zero;
   } GimbalData;
 
diff --git a/src/module/gimbal/mod_gimbal.cpp b/src/module/gimbal/mod_gimbal.cpp
index 62ffa5cf..fcec4e56 100644
--- a/src/module/gimbal/mod_gimbal.cpp
+++ b/src/module/gimbal/mod_gimbal.cpp
@@ -81,11 +81,10 @@ void Gimbal::UpdateFeedback() {
   switch (this->mode_) {
     case RELAX:
     case ABSOLUTE:
-      this->gimbal_data_.yaw =
+      this->gimbal_data_.yaw_for_chassis =
           this->yaw_motor_.GetAngle() - this->param_.mech_zero.yaw;
       break;
   }
-  this->gimbal_data_.mech_zero = this->param_.mech_zero;
   this->gimbal_data_.scanf_mode = this->param_.scanf_mode;
 }
 
@@ -109,7 +108,6 @@ void Gimbal::Control() {
     gimbal_pit_cmd = Component::Type::CycleValue(this->cmd_.eulr.pit) -
                      this->setpoint_.eulr_.pit;
   }
-
   /* 处理yaw控制命令，软件限位  */
   /* 某个轴max=min时不进行限位,配置文件默认不写 */
   if (param_.limit.yaw_max != param_.limit.yaw_min) {
@@ -253,9 +251,9 @@ void Gimbal::DrawUIStatic(Gimbal* gimbal) {
       static_cast<uint16_t>(Device::Referee::UIGetWidth() * 0.4f),
       static_cast<uint16_t>(Device::Referee::UIGetHeight() * 0.2f),
       static_cast<uint16_t>(Device::Referee::UIGetWidth() * 0.4f +
-                            -sinf(gimbal->gimbal_data_.yaw) * 44),
+                            -sinf(gimbal->gimbal_data_.yaw_for_chassis) * 44),
       static_cast<uint16_t>(Device::Referee::UIGetHeight() * 0.2f +
-                            cosf(gimbal->gimbal_data_.yaw) * 44));
+                            cosf(gimbal->gimbal_data_.yaw_for_chassis) * 44));
   Device::Referee::AddUI(gimbal->line_);
 }
 
@@ -308,8 +306,8 @@ void Gimbal::DrawUIDynamic(Gimbal* gimbal) {
       static_cast<uint16_t>(Device::Referee::UIGetWidth() * 0.4f),
       static_cast<uint16_t>(Device::Referee::UIGetHeight() * 0.2f),
       static_cast<uint16_t>(Device::Referee::UIGetWidth() * 0.4f +
-                            -sinf(gimbal->gimbal_data_.yaw) * 44),
+                            -sinf(gimbal->gimbal_data_.yaw_for_chassis) * 44),
       static_cast<uint16_t>(Device::Referee::UIGetHeight() * 0.2f +
-                            cosf(gimbal->gimbal_data_.yaw) * 44));
+                            cosf(gimbal->gimbal_data_.yaw_for_chassis) * 44));
   Device::Referee::AddUI(gimbal->line_);
 }
diff --git a/src/module/gimbal/mod_gimbal.hpp b/src/module/gimbal/mod_gimbal.hpp
index e3d72380..55f50b74 100644
--- a/src/module/gimbal/mod_gimbal.hpp
+++ b/src/module/gimbal/mod_gimbal.hpp
@@ -45,8 +45,7 @@ class Gimbal {
     float scanf_pit_omega;
   } ScanfMode;
   typedef struct {
-    float yaw;
-    Component::Type::Eulr mech_zero;
+    float yaw_for_chassis;
     ScanfMode scanf_mode;
   } GimbalData;
 
diff --git a/src/robot/sentry/robot.cpp b/src/robot/sentry/robot.cpp
index 0719f003..ee89ef37 100644
--- a/src/robot/sentry/robot.cpp
+++ b/src/robot/sentry/robot.cpp
@@ -234,14 +234,14 @@ Robot::Sentry::Param param = {
 
     .mech_zero = {
       .yaw = 1.58f,
-      .pit = 4.7f,
+      .pit = 4.6f,
       .rol = 0.0f,
     },
     .scanf_mode{
-      .scanf_yaw_rate = 0.0f,
-      .scanf_pit_center = 4.7f,
-      .scanf_pit_range = 0.3f,
-      .scanf_pit_omega = 2.0f,
+      .scanf_yaw_rate = 0.005f,
+      .scanf_pit_center = 0.04f,
+      .scanf_pit_range = 0.22f,
+      .scanf_pit_omega = 5.0f,
     },
 
     .limit = {
-- 
Gitee


From e966a8367026c3efb4baa4ca6fe46bda5ce16015 Mon Sep 17 00:00:00 2001
From: Marine_zxy <1423266892@qq.com>
Date: Thu, 9 May 2024 15:54:40 +0800
Subject: [PATCH 11/19] =?UTF-8?q?=E5=AE=8C=E5=96=84=E6=9B=B4=E6=94=B9?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/device/ai/dev_ai.cpp | 127 ++++++++++++++++++++-------------------
 1 file changed, 64 insertions(+), 63 deletions(-)

diff --git a/src/device/ai/dev_ai.cpp b/src/device/ai/dev_ai.cpp
index 6361e53f..91d1db4e 100644
--- a/src/device/ai/dev_ai.cpp
+++ b/src/device/ai/dev_ai.cpp
@@ -230,73 +230,74 @@ bool AI::PackCMD() {
 
   /* 控制权在AI */
   if (Component::CMD::GetCtrlSource() == Component::CMD::CTRL_SOURCE_AI) {
-    // if (this->cmd_.online) { /* AI在线 */
-    switch (this->action_.ai_chassis) {
-      case TO_BASE:
-      case TO_OUTPOST:
-      case TO_SUPPLY:
-      case TO_HIGHLAND:
-      case TO_PATROL_AREA:
-        memcpy(&(this->cmd_.chassis), &(this->from_host_.data.chassis_move_vec),
-               sizeof(this->from_host_.data.chassis_move_vec));
-        break;
-      case ROTOR:
-        this->cmd_.chassis.x = 0;
-        this->cmd_.chassis.y = 0;
-        this->cmd_.chassis.z = 0.5;  //待修改
-        break;
-    }
+    if (this->cmd_.online) { /* AI在线 */
+      switch (this->action_.ai_chassis) {
+        case TO_BASE:
+        case TO_OUTPOST:
+        case TO_SUPPLY:
+        case TO_HIGHLAND:
+        case TO_PATROL_AREA:
+          memcpy(&(this->cmd_.chassis),
+                 &(this->from_host_.data.chassis_move_vec),
+                 sizeof(this->from_host_.data.chassis_move_vec));
+          break;
+        case ROTOR:
+          this->cmd_.chassis.x = 0;
+          this->cmd_.chassis.y = 0;
+          this->cmd_.chassis.z = 0.5;  //待修改
+          break;
+      }
 
-    switch (this->action_.ai_gimbal) {
-      case AUTO_AIM:
-        memcpy(&(this->cmd_.gimbal.eulr), &(this->from_host_.data.gimbal),
-               sizeof(this->cmd_.gimbal.eulr));
-        break;
-      case AFFECTED:
-        this->cmd_.gimbal.eulr.yaw = this->damaged_id_ * (M_2PI / 4.0);
-        this->cmd_.gimbal.eulr.pit = 4.5f;  //待修改成合适角度
-        break;
-      case SCANF:
-        this->delta_angle_ += this->gimbal_data_.scanf_mode.scanf_yaw_rate;
-        this->cmd_.gimbal.eulr.yaw = this->delta_angle_;
-
-        this->cmd_.gimbal.eulr.pit =
-            this->gimbal_data_.scanf_mode.scanf_pit_center +
-            this->gimbal_data_.scanf_mode.scanf_pit_range *
-                sin(this->gimbal_data_.scanf_mode.scanf_pit_omega *
-                    bsp_time_get_ms() / 1000.0);
-        break;
-    }
+      switch (this->action_.ai_gimbal) {
+        case AUTO_AIM:
+          memcpy(&(this->cmd_.gimbal.eulr), &(this->from_host_.data.gimbal),
+                 sizeof(this->cmd_.gimbal.eulr));
+          break;
+        case AFFECTED:
+          this->cmd_.gimbal.eulr.yaw = this->damaged_id_ * (M_2PI / 4.0);
+          this->cmd_.gimbal.eulr.pit = 4.5f;  //待修改成合适角度
+          break;
+        case SCANF:
+          this->delta_angle_ += this->gimbal_data_.scanf_mode.scanf_yaw_rate;
+          this->cmd_.gimbal.eulr.yaw = this->delta_angle_;
+
+          this->cmd_.gimbal.eulr.pit =
+              this->gimbal_data_.scanf_mode.scanf_pit_center +
+              this->gimbal_data_.scanf_mode.scanf_pit_range *
+                  sin(this->gimbal_data_.scanf_mode.scanf_pit_omega *
+                      bsp_time_get_ms() / 1000.0);
+          break;
+      }
 
-    switch (this->action_.ai_launcher) {
-      case FIRE:
-        this->event_.Active(AI_FIRE_COMMAND);  //写热量控制！！最大化利用
-      case CEASEFIRE:
-        break;
-    }
+      switch (this->action_.ai_launcher) {
+        case FIRE:
+          this->event_.Active(AI_FIRE_COMMAND);  //写热量控制！！最大化利用
+        case CEASEFIRE:
+          break;
+      }
 
-    switch (this->action_.ai_to_referee) {  //需要和referee.cpp产生联系
-      case CONFIRM_RESURRECTION:
-        cmd_for_ref_.confirm_resurrection = 1;
-        cmd_for_ref_.buy_resurrection = 0;
-        cmd_for_ref_.buy_bullet_num = this->last_buy_bullet_num_;
-        cmd_for_ref_.remote_buy_bullet_times = 0;
-        cmd_for_ref_.romote_buy_hp_times = 0;
-        break;
-      case EXCHANGE_BULLETS:
-        cmd_for_ref_.confirm_resurrection = 0;
-        cmd_for_ref_.buy_resurrection = 0;
-        cmd_for_ref_.buy_bullet_num = this->last_buy_bullet_num_ + 50;
-        cmd_for_ref_.remote_buy_bullet_times = 0;
-        cmd_for_ref_.romote_buy_hp_times = 0;
-        break;
-      case NOTHING:
-        break;
+      switch (this->action_.ai_to_referee) {  //需要和referee.cpp产生联系
+        case CONFIRM_RESURRECTION:
+          cmd_for_ref_.confirm_resurrection = 1;
+          cmd_for_ref_.buy_resurrection = 0;
+          cmd_for_ref_.buy_bullet_num = this->last_buy_bullet_num_;
+          cmd_for_ref_.remote_buy_bullet_times = 0;
+          cmd_for_ref_.romote_buy_hp_times = 0;
+          break;
+        case EXCHANGE_BULLETS:
+          cmd_for_ref_.confirm_resurrection = 0;
+          cmd_for_ref_.buy_resurrection = 0;
+          cmd_for_ref_.buy_bullet_num = this->last_buy_bullet_num_ + 50;
+          cmd_for_ref_.remote_buy_bullet_times = 0;
+          cmd_for_ref_.romote_buy_hp_times = 0;
+          break;
+        case NOTHING:
+          break;
+      }
+      this->last_buy_bullet_num_ = cmd_for_ref_.buy_bullet_num;
+    } else {
+      this->event_.Active(AI_OFFLINE);  //这个逻辑是对的
     }
-    this->last_buy_bullet_num_ = cmd_for_ref_.buy_bullet_num;
-    // } else {
-    // this->event_.Active(AI_OFFLINE);  //这个逻辑是对的
-    // }
   }
 
   // memcpy(&(this->last_eulr_), &(this->cmd_.gimbal.eulr),
-- 
Gitee


From 5288c6075dbc616e8f7158724ab71170f0f7b11c Mon Sep 17 00:00:00 2001
From: Marine_zxy <1423266892@qq.com>
Date: Thu, 9 May 2024 16:16:43 +0800
Subject: [PATCH 12/19] =?UTF-8?q?ai=E9=80=82=E9=85=8D=E4=B8=A4=E7=A7=8D?=
 =?UTF-8?q?=E6=8E=A7=E5=88=B6=E6=A8=A1=E5=BC=8F?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/component/comp_cmd.hpp |   1 +
 src/device/ai/dev_ai.cpp   | 152 ++++++++++++++++++++-----------------
 src/device/ai/dev_ai.hpp   |   5 --
 3 files changed, 85 insertions(+), 73 deletions(-)

diff --git a/src/component/comp_cmd.hpp b/src/component/comp_cmd.hpp
index 5b4111ee..be3b97e0 100644
--- a/src/component/comp_cmd.hpp
+++ b/src/component/comp_cmd.hpp
@@ -101,6 +101,7 @@ class CMD {
     self_->ctrl_source_ = source;
   }
   static ControlSource GetCtrlSource() { return self_->ctrl_source_; }
+  static Mode GetCtrlMode() { return self_->mode_; }
 
   static bool Online() { return self_->online_; }
 
diff --git a/src/device/ai/dev_ai.cpp b/src/device/ai/dev_ai.cpp
index 91d1db4e..08148600 100644
--- a/src/device/ai/dev_ai.cpp
+++ b/src/device/ai/dev_ai.cpp
@@ -230,80 +230,96 @@ bool AI::PackCMD() {
 
   /* 控制权在AI */
   if (Component::CMD::GetCtrlSource() == Component::CMD::CTRL_SOURCE_AI) {
-    if (this->cmd_.online) { /* AI在线 */
-      switch (this->action_.ai_chassis) {
-        case TO_BASE:
-        case TO_OUTPOST:
-        case TO_SUPPLY:
-        case TO_HIGHLAND:
-        case TO_PATROL_AREA:
-          memcpy(&(this->cmd_.chassis),
-                 &(this->from_host_.data.chassis_move_vec),
-                 sizeof(this->from_host_.data.chassis_move_vec));
-          break;
-        case ROTOR:
-          this->cmd_.chassis.x = 0;
-          this->cmd_.chassis.y = 0;
-          this->cmd_.chassis.z = 0.5;  //待修改
-          break;
+    if (Component::CMD::GetCtrlMode() == Component::CMD::CMD_AUTO_CTRL) {
+      if (this->cmd_.online) { /* AI在线 */
+        switch (this->action_.ai_chassis) {
+          case TO_BASE:
+          case TO_OUTPOST:
+          case TO_SUPPLY:
+          case TO_HIGHLAND:
+          case TO_PATROL_AREA:
+            memcpy(&(this->cmd_.chassis),
+                   &(this->from_host_.data.chassis_move_vec),
+                   sizeof(this->from_host_.data.chassis_move_vec));
+            break;
+          case ROTOR:
+            this->cmd_.chassis.x = 0;
+            this->cmd_.chassis.y = 0;
+            this->cmd_.chassis.z = 0.5;  //待修改
+            break;
+        }
+
+        switch (this->action_.ai_gimbal) {
+          case AUTO_AIM:
+            memcpy(&(this->cmd_.gimbal.eulr), &(this->from_host_.data.gimbal),
+                   sizeof(this->cmd_.gimbal.eulr));
+            break;
+          case AFFECTED:
+            this->cmd_.gimbal.eulr.yaw = this->damaged_id_ * (M_2PI / 4.0);
+            this->cmd_.gimbal.eulr.pit = 4.5f;  //待修改成合适角度
+            break;
+          case SCANF:
+            this->delta_angle_ += this->gimbal_data_.scanf_mode.scanf_yaw_rate;
+            this->cmd_.gimbal.eulr.yaw = this->delta_angle_;
+
+            this->cmd_.gimbal.eulr.pit =
+                this->gimbal_data_.scanf_mode.scanf_pit_center +
+                this->gimbal_data_.scanf_mode.scanf_pit_range *
+                    sin(this->gimbal_data_.scanf_mode.scanf_pit_omega *
+                        bsp_time_get_ms() / 1000.0);
+            break;
+        }
+
+        switch (this->action_.ai_launcher) {
+          case FIRE:
+            this->event_.Active(AI_FIRE_COMMAND);  //写热量控制！！最大化利用
+          case CEASEFIRE:
+            break;
+        }
+
+        switch (this->action_.ai_to_referee) {  //需要和referee.cpp产生联系
+          case CONFIRM_RESURRECTION:
+            cmd_for_ref_.confirm_resurrection = 1;
+            cmd_for_ref_.buy_resurrection = 0;
+            cmd_for_ref_.buy_bullet_num = this->last_buy_bullet_num_;
+            cmd_for_ref_.remote_buy_bullet_times = 0;
+            cmd_for_ref_.romote_buy_hp_times = 0;
+            break;
+          case EXCHANGE_BULLETS:
+            cmd_for_ref_.confirm_resurrection = 0;
+            cmd_for_ref_.buy_resurrection = 0;
+            cmd_for_ref_.buy_bullet_num = this->last_buy_bullet_num_ + 50;
+            cmd_for_ref_.remote_buy_bullet_times = 0;
+            cmd_for_ref_.romote_buy_hp_times = 0;
+            break;
+          case NOTHING:
+            break;
+        }
+        this->last_buy_bullet_num_ = cmd_for_ref_.buy_bullet_num;
+      } else {
+        this->event_.Active(AI_OFFLINE);  //这个逻辑是对的
       }
+    }
+    this->notice_ = 0;
+    this->cmd_tp_.Publish(this->cmd_);
+  } else if (Component::CMD::GetCtrlMode() == Component::CMD::CMD_OP_CTRL) {
+    memcpy(&(this->cmd_.gimbal.eulr), &(this->from_host_.data.gimbal),
+           sizeof(this->cmd_.gimbal.eulr));
 
-      switch (this->action_.ai_gimbal) {
-        case AUTO_AIM:
-          memcpy(&(this->cmd_.gimbal.eulr), &(this->from_host_.data.gimbal),
-                 sizeof(this->cmd_.gimbal.eulr));
-          break;
-        case AFFECTED:
-          this->cmd_.gimbal.eulr.yaw = this->damaged_id_ * (M_2PI / 4.0);
-          this->cmd_.gimbal.eulr.pit = 4.5f;  //待修改成合适角度
-          break;
-        case SCANF:
-          this->delta_angle_ += this->gimbal_data_.scanf_mode.scanf_yaw_rate;
-          this->cmd_.gimbal.eulr.yaw = this->delta_angle_;
-
-          this->cmd_.gimbal.eulr.pit =
-              this->gimbal_data_.scanf_mode.scanf_pit_center +
-              this->gimbal_data_.scanf_mode.scanf_pit_range *
-                  sin(this->gimbal_data_.scanf_mode.scanf_pit_omega *
-                      bsp_time_get_ms() / 1000.0);
-          break;
-      }
+    memcpy(&(this->cmd_.ext.extern_channel),
+           &(this->from_host_.data.extern_channel),
+           sizeof(this->cmd_.ext.extern_channel));
 
-      switch (this->action_.ai_launcher) {
-        case FIRE:
-          this->event_.Active(AI_FIRE_COMMAND);  //写热量控制！！最大化利用
-        case CEASEFIRE:
-          break;
-      }
+    memcpy(&(this->cmd_.chassis), &(this->from_host_.data.chassis_move_vec),
+           sizeof(this->from_host_.data.chassis_move_vec));
 
-      switch (this->action_.ai_to_referee) {  //需要和referee.cpp产生联系
-        case CONFIRM_RESURRECTION:
-          cmd_for_ref_.confirm_resurrection = 1;
-          cmd_for_ref_.buy_resurrection = 0;
-          cmd_for_ref_.buy_bullet_num = this->last_buy_bullet_num_;
-          cmd_for_ref_.remote_buy_bullet_times = 0;
-          cmd_for_ref_.romote_buy_hp_times = 0;
-          break;
-        case EXCHANGE_BULLETS:
-          cmd_for_ref_.confirm_resurrection = 0;
-          cmd_for_ref_.buy_resurrection = 0;
-          cmd_for_ref_.buy_bullet_num = this->last_buy_bullet_num_ + 50;
-          cmd_for_ref_.remote_buy_bullet_times = 0;
-          cmd_for_ref_.romote_buy_hp_times = 0;
-          break;
-        case NOTHING:
-          break;
-      }
-      this->last_buy_bullet_num_ = cmd_for_ref_.buy_bullet_num;
-    } else {
-      this->event_.Active(AI_OFFLINE);  //这个逻辑是对的
-    }
+    this->notice_ = this->from_host_.data.notice;
+
+    this->cmd_.ctrl_source = Component::CMD::CTRL_SOURCE_AI;
+
+    this->cmd_tp_.Publish(this->cmd_);
   }
 
-  // memcpy(&(this->last_eulr_), &(this->cmd_.gimbal.eulr),
-  //        sizeof(this->cmd_.gimbal.eulr));
-  this->notice_ = 0;
-  this->cmd_tp_.Publish(this->cmd_);
   return true;
 }
 
diff --git a/src/device/ai/dev_ai.hpp b/src/device/ai/dev_ai.hpp
index f25d4e97..4bceb330 100644
--- a/src/device/ai/dev_ai.hpp
+++ b/src/device/ai/dev_ai.hpp
@@ -159,11 +159,6 @@ class AI {
 
   Message::Topic<SentryDecisionData> ai_tp_ =
       Message::Topic<SentryDecisionData>("sentry_cmd_for_ref");
-  // struct {
-  //   float yaw; /* 偏航角（Yaw angle） */
-  //   float pit; /* 俯仰角（Pitch angle） */
-  //   float rol; /* 翻滚角（Roll angle） */
-  // } last_eulr_;
 
   Component::Type::Quaternion quat_{};
   Device::Referee::Data raw_ref_{};
-- 
Gitee


From 8ef02ac0e867062dc2bd01becd4bc72dff11f9e6 Mon Sep 17 00:00:00 2001
From: Marine_zxy <1423266892@qq.com>
Date: Fri, 10 May 2024 10:40:13 +0800
Subject: [PATCH 13/19] =?UTF-8?q?=E5=93=A8=E5=85=B5=E5=8F=97=E5=87=BB?=
 =?UTF-8?q?=E5=8F=8D=E9=A6=88=E5=AE=8C=E6=88=90?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/device/ai/dev_ai.cpp           | 82 +++++++++++++++++++-----------
 src/device/ai/dev_ai.hpp           | 17 ++++++-
 src/device/referee/dev_referee.cpp | 30 +++++++----
 src/device/referee/dev_referee.hpp |  6 +++
 src/module/gimbal/mod_gimbal.cpp   |  1 +
 src/module/gimbal/mod_gimbal.hpp   |  1 +
 src/robot/sentry/robot.cpp         |  2 +-
 7 files changed, 97 insertions(+), 42 deletions(-)

diff --git a/src/device/ai/dev_ai.cpp b/src/device/ai/dev_ai.cpp
index 08148600..564557f5 100644
--- a/src/device/ai/dev_ai.cpp
+++ b/src/device/ai/dev_ai.cpp
@@ -158,6 +158,7 @@ void AI::DecideAction() {
   if (this->raw_ref_.robot_damage.damage_type == 0x0 && !last_damage_type_) {
     this->is_damaged_ = true;
     this->damaged_id_ = this->raw_ref_.robot_damage.armor_id;
+    this->damaged_time_ = bsp_time_get_ms();
   }
   this->last_damage_type_ = this->raw_ref_.robot_damage.damage_type;
 
@@ -193,7 +194,9 @@ void AI::DecideAction() {
   } else if (is_damaged_) { /* 反击 */
     this->cmd_.gimbal.mode = Component::CMD::GIMBAL_ABSOLUTE_CTRL;
     this->action_.ai_gimbal = AI::Action::AFFECTED;
-    is_damaged_ = false;
+    if (bsp_time_get_ms() - this->damaged_time_ > 1000) {
+      is_damaged_ = false;
+    }
   } else { /* 扫描 */
     this->cmd_.gimbal.mode = Component::CMD::GIMBAL_ABSOLUTE_CTRL;
     this->action_.ai_gimbal = AI::Action::SCANF;
@@ -257,16 +260,23 @@ bool AI::PackCMD() {
           case AFFECTED:
             this->cmd_.gimbal.eulr.yaw = this->damaged_id_ * (M_2PI / 4.0);
             this->cmd_.gimbal.eulr.pit = 4.5f;  //待修改成合适角度
+            this->last_yaw_angle_ = this->gimbal_data_.get_yaw_angle - 1.58;
+            this->target_angle_ = 0.0;
+
             break;
           case SCANF:
-            this->delta_angle_ += this->gimbal_data_.scanf_mode.scanf_yaw_rate;
-            this->cmd_.gimbal.eulr.yaw = this->delta_angle_;
-
-            this->cmd_.gimbal.eulr.pit =
-                this->gimbal_data_.scanf_mode.scanf_pit_center +
-                this->gimbal_data_.scanf_mode.scanf_pit_range *
-                    sin(this->gimbal_data_.scanf_mode.scanf_pit_omega *
-                        bsp_time_get_ms() / 1000.0);
+            // this->target_angle_ +=
+            // this->gimbal_data_.scanf_mode.scanf_yaw_rate;
+            // this->cmd_.gimbal.eulr.yaw =
+            //     this->last_yaw_angle_ + this->target_angle_;
+
+            // this->cmd_.gimbal.eulr.pit =
+            //     this->gimbal_data_.scanf_mode.scanf_pit_center +
+            //     this->gimbal_data_.scanf_mode.scanf_pit_range *
+            //         sin(this->gimbal_data_.scanf_mode.scanf_pit_omega *
+            //             bsp_time_get_ms() / 1000.0);
+            this->cmd_.gimbal.eulr.pit = 4.5;
+            this->cmd_.gimbal.eulr.yaw = 0;
             break;
         }
 
@@ -277,49 +287,57 @@ bool AI::PackCMD() {
             break;
         }
 
-        switch (this->action_.ai_to_referee) {  //需要和referee.cpp产生联系
+        switch (this->action_.ai_to_referee) {
           case CONFIRM_RESURRECTION:
             cmd_for_ref_.confirm_resurrection = 1;
-            cmd_for_ref_.buy_resurrection = 0;
-            cmd_for_ref_.buy_bullet_num = this->last_buy_bullet_num_;
+            cmd_for_ref_.buy_resurrection = 1;
+            cmd_for_ref_.buy_bullet_num = 0;
             cmd_for_ref_.remote_buy_bullet_times = 0;
             cmd_for_ref_.romote_buy_hp_times = 0;
             break;
           case EXCHANGE_BULLETS:
+            // cmd_for_ref_.confirm_resurrection = 0;
+            // cmd_for_ref_.buy_resurrection = 0;
+            // cmd_for_ref_.buy_bullet_num = this->last_buy_bullet_num_ + 50;
+            // cmd_for_ref_.remote_buy_bullet_times = 0;
+            // cmd_for_ref_.romote_buy_hp_times = 0;
+            break;
+          case NOTHING:
             cmd_for_ref_.confirm_resurrection = 0;
             cmd_for_ref_.buy_resurrection = 0;
-            cmd_for_ref_.buy_bullet_num = this->last_buy_bullet_num_ + 50;
-            cmd_for_ref_.remote_buy_bullet_times = 0;
+            cmd_for_ref_.buy_bullet_num = this->last_buy_bullet_num_;
+            cmd_for_ref_.remote_buy_bullet_times = 2;
             cmd_for_ref_.romote_buy_hp_times = 0;
             break;
-          case NOTHING:
-            break;
         }
         this->last_buy_bullet_num_ = cmd_for_ref_.buy_bullet_num;
       } else {
-        this->event_.Active(AI_OFFLINE);  //这个逻辑是对的
+        this->event_.Active(AI_OFFLINE);
       }
+      this->notice_ = 0;
+      this->cmd_tp_.Publish(this->cmd_);
     }
-    this->notice_ = 0;
-    this->cmd_tp_.Publish(this->cmd_);
-  } else if (Component::CMD::GetCtrlMode() == Component::CMD::CMD_OP_CTRL) {
-    memcpy(&(this->cmd_.gimbal.eulr), &(this->from_host_.data.gimbal),
-           sizeof(this->cmd_.gimbal.eulr));
 
-    memcpy(&(this->cmd_.ext.extern_channel),
-           &(this->from_host_.data.extern_channel),
-           sizeof(this->cmd_.ext.extern_channel));
+    else if (Component::CMD::GetCtrlMode() == Component::CMD::CMD_OP_CTRL) {
+      memcpy(&(this->cmd_.gimbal.eulr), &(this->from_host_.data.gimbal),
+             sizeof(this->cmd_.gimbal.eulr));
 
-    memcpy(&(this->cmd_.chassis), &(this->from_host_.data.chassis_move_vec),
-           sizeof(this->from_host_.data.chassis_move_vec));
+      memcpy(&(this->cmd_.ext.extern_channel),
+             &(this->from_host_.data.extern_channel),
+             sizeof(this->cmd_.ext.extern_channel));
 
-    this->notice_ = this->from_host_.data.notice;
+      memcpy(&(this->cmd_.chassis), &(this->from_host_.data.chassis_move_vec),
+             sizeof(this->from_host_.data.chassis_move_vec));
 
-    this->cmd_.ctrl_source = Component::CMD::CTRL_SOURCE_AI;
+      this->notice_ = this->from_host_.data.notice;
 
-    this->cmd_tp_.Publish(this->cmd_);
-  }
+      this->cmd_.ctrl_source = Component::CMD::CTRL_SOURCE_AI;
 
+      this->cmd_tp_.Publish(this->cmd_);
+      this->cmd_tp_.Publish(this->cmd_);
+      this->cmd_tp_.Publish(this->cmd_);
+    }
+  }
   return true;
 }
 
@@ -387,6 +405,8 @@ void AI::PraseRef() {
       this->ref_.robot_id = AI_ARM_INFANTRY;
   }
 
+  this->ref_.game_progress = this->raw_ref_.game_status.game_progress;
+
   if (this->raw_ref_.robot_status.robot_id < 100) {
     this->ref_.base_hp = this->raw_ref_.game_robot_hp.red_base;
     this->ref_.outpost_hp = this->raw_ref_.game_robot_hp.red_outpose;
diff --git a/src/device/ai/dev_ai.hpp b/src/device/ai/dev_ai.hpp
index 4bceb330..b0e5dc20 100644
--- a/src/device/ai/dev_ai.hpp
+++ b/src/device/ai/dev_ai.hpp
@@ -30,6 +30,7 @@ class AI {
     uint32_t max_hp;
     uint32_t hp;
 
+    uint8_t game_progress;
     uint16_t base_hp;
     uint16_t outpost_hp;
     uint16_t bullet_num;
@@ -51,6 +52,7 @@ class AI {
   typedef struct {
     float yaw_for_chassis;
     ScanfMode scanf_mode;
+    float get_yaw_angle;
   } GimbalData;
 
   typedef enum {
@@ -60,6 +62,15 @@ class AI {
     AI_TURN,
     AI_FIRE_COMMAND,
   } AIControlData;
+
+  typedef enum {
+    WAITTING = 0,
+    PREPARATION = 1,
+    REF_INSPECTION = 2,
+    COUNTDOWN_5S = 3,
+    GAMING = 4,
+    GAME_END = 5,
+  } GameProgress;
   typedef enum {
     TO_BASE,        // 0
     TO_OUTPOST,     // 1
@@ -133,9 +144,11 @@ class AI {
   } to_host_;
 
   RefForAI ref_{};
+
   uint8_t last_damage_type_;
   bool is_damaged_;
   uint8_t damaged_id_;
+  uint32_t damaged_time_;
 
   System::Thread thread_;
 
@@ -153,9 +166,11 @@ class AI {
 
   GimbalData gimbal_data_;
 
+  float last_yaw_angle_;
+
   SentryDecisionData cmd_for_ref_;
   uint8_t last_buy_bullet_num_ = 0;
-  Component::Type::CycleValue delta_angle_ = 0.0;
+  Component::Type::CycleValue target_angle_ = 0.0;
 
   Message::Topic<SentryDecisionData> ai_tp_ =
       Message::Topic<SentryDecisionData>("sentry_cmd_for_ref");
diff --git a/src/device/referee/dev_referee.cpp b/src/device/referee/dev_referee.cpp
index 78080384..124c10bd 100644
--- a/src/device/referee/dev_referee.cpp
+++ b/src/device/referee/dev_referee.cpp
@@ -101,7 +101,7 @@ Referee::Referee() : event_(Message::Event::FindEvent("cmd_event")) {
     uint32_t last_online_time = bsp_time_get_ms();
     auto ai_sub = Message::Subscriber<SentryDecisionData>("sentry_cmd_for_ref");
     while (1) {
-      ai_sub.DumpData(ref->sentry_pack_.sentry_cmd);
+      ai_sub.DumpData(ref->data_from_sentry_);
       ref->UpdateUI(); /* 更新UI */
 
       if (ref->ref_data_.robot_status.robot_id == REF_BOT_RED_SENTRY ||
@@ -496,26 +496,31 @@ bool Referee::UpdateUI() {
 
 bool Referee::SentryDecision() {
   uint32_t pack_size = 0;
-  pack_size = sizeof(SentryPack);
+  pack_size = sizeof(InterStudentHeader) + sizeof(SentryDecisionData);
   this->packet_sent_.Wait(UINT32_MAX);  //上锁
 
   this->sentry_lock_.Wait(UINT32_MAX);  //上锁
 
-  this->sentry_pack_.cmd_id = REF_CMD_ID_INTER_STUDENT;  // 0x0301
+  SetPacketHeader(this->sentry_pack_.frame_header, pack_size);
 
-  // this->sentry_pack_.sentry_cmd 经过topic获得数据
+  this->sentry_pack_.cmd_id = REF_CMD_ID_INTER_STUDENT;  // 0x0301
 
-  SetUIHeader(this->sentry_pack_.student_header, REF_STDNT_CMD_ID_SENTRY_CMD,
-              static_cast<RobotID>(this->ref_data_.robot_status.robot_id));
+  SetSentryHeader(this->sentry_pack_.student_header,
+                  REF_STDNT_CMD_ID_SENTRY_CMD,
+                  static_cast<RobotID>(this->ref_data_.robot_status.robot_id));
   //设置子ID、发送者、接受者
 
-  SetPacketHeader(this->sentry_pack_.frame_header, pack_size - 9);
-  //设置通信协议
+  this->sentry_pack_.sentry_cmd = this->data_from_sentry_;
+
+  this->sentry_pack_.crc16 = Component::CRC16::Calculate(
+      reinterpret_cast<const uint8_t *>(&this->sentry_pack_),
+      pack_size - sizeof(uint16_t), CRC16_INIT);
+
   bsp_uart_transmit(BSP_UART_REF,
                     reinterpret_cast<uint8_t *>(&this->sentry_pack_), pack_size,
                     false);
+
   this->sentry_lock_.Post();
-  this->packet_sent_.Post();
   return true;
 }
 
@@ -562,6 +567,13 @@ void Referee::SetUIHeader(Referee::InterStudentHeader &header,
     header.id_receiver = robot_id + 0x0100;
   }
 }
+void Referee::SetSentryHeader(Referee::InterStudentHeader &header,
+                              const Referee::CMDID CMD_ID,
+                              Referee::RobotID robot_id) {
+  header.cmd_id = CMD_ID;
+  header.id_sender = robot_id;
+  header.id_receiver = 0x8080;
+}
 
 void Referee::SetPacketHeader(Referee::Header &header, uint16_t data_length) {
   static uint8_t seq = 0;
diff --git a/src/device/referee/dev_referee.hpp b/src/device/referee/dev_referee.hpp
index 0a43349d..b5042c00 100644
--- a/src/device/referee/dev_referee.hpp
+++ b/src/device/referee/dev_referee.hpp
@@ -508,8 +508,10 @@ class Referee {
   typedef struct __attribute__((packed)) {
     Header frame_header;  // 0x0301
     uint16_t cmd_id;
+
     Referee::InterStudentHeader student_header;  //含0x0102
     SentryDecisionData sentry_cmd;
+
     uint16_t crc16;
   } SentryPack;
 
@@ -548,6 +550,8 @@ class Referee {
 
   void SetUIHeader(InterStudentHeader &header, const CMDID CMD_ID,
                    RobotID robot_id);
+  void SetSentryHeader(InterStudentHeader &header, const CMDID CMD_ID,
+                       RobotID robot_id);
 
   void SetPacketHeader(Referee::Header &header, uint16_t data_length);
 
@@ -589,8 +593,10 @@ class Referee {
   Message::Event event_;
 
   static UIPack ui_pack_;
+
   SentryPack sentry_pack_;
 
   static Referee *self_;
+  SentryDecisionData data_from_sentry_;
 };
 }  // namespace Device
diff --git a/src/module/gimbal/mod_gimbal.cpp b/src/module/gimbal/mod_gimbal.cpp
index fcec4e56..3be23c90 100644
--- a/src/module/gimbal/mod_gimbal.cpp
+++ b/src/module/gimbal/mod_gimbal.cpp
@@ -86,6 +86,7 @@ void Gimbal::UpdateFeedback() {
       break;
   }
   this->gimbal_data_.scanf_mode = this->param_.scanf_mode;
+  this->gimbal_data_.get_yaw_angle = this->yaw_motor_.GetAngle();
 }
 
 void Gimbal::Control() {
diff --git a/src/module/gimbal/mod_gimbal.hpp b/src/module/gimbal/mod_gimbal.hpp
index 55f50b74..ae7eadda 100644
--- a/src/module/gimbal/mod_gimbal.hpp
+++ b/src/module/gimbal/mod_gimbal.hpp
@@ -47,6 +47,7 @@ class Gimbal {
   typedef struct {
     float yaw_for_chassis;
     ScanfMode scanf_mode;
+    float get_yaw_angle;
   } GimbalData;
 
   typedef struct {
diff --git a/src/robot/sentry/robot.cpp b/src/robot/sentry/robot.cpp
index ee89ef37..5fc98f80 100644
--- a/src/robot/sentry/robot.cpp
+++ b/src/robot/sentry/robot.cpp
@@ -204,7 +204,7 @@ Robot::Sentry::Param param = {
 
         .position = {
           /* GIMBAL_CTRL_PIT_ANGLE_IDX */
-          .k = 33.0f,
+          .k = 25.0f,
           .p = 1.0f,
           .i = 0.0f,
           .d = 0.0f,
-- 
Gitee


From 4fca937a2f0a2de65a6ef46fc7ee2bcefd475ae0 Mon Sep 17 00:00:00 2001
From: Marine_zxy <1423266892@qq.com>
Date: Mon, 13 May 2024 21:47:40 +0800
Subject: [PATCH 14/19] =?UTF-8?q?=E5=AE=8C=E5=96=84?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/device/ai/dev_ai.cpp             | 230 +++++++++++++++------------
 src/device/ai/dev_ai.hpp             |  26 ++-
 src/device/referee/dev_referee.cpp   |  76 +++++++--
 src/device/referee/dev_referee.hpp   |  12 +-
 src/module/gimbal/mod_gimbal.cpp     |   1 -
 src/module/launcher/mod_launcher.cpp |  30 +++-
 src/module/launcher/mod_launcher.hpp |   6 +-
 src/robot/sentry/robot.cpp           |  49 ++++--
 src/robot/sentry/robot.hpp           |   2 +
 9 files changed, 283 insertions(+), 149 deletions(-)

diff --git a/src/device/ai/dev_ai.cpp b/src/device/ai/dev_ai.cpp
index 564557f5..dce01468 100644
--- a/src/device/ai/dev_ai.cpp
+++ b/src/device/ai/dev_ai.cpp
@@ -16,8 +16,9 @@ static uint8_t txbuf[AI_LEN_TX_BUFF];
 
 using namespace Device;
 
-AI::AI()
-    : data_ready_(false),
+AI::AI(bool autoscan_enable)
+    : autoscan_enable_(autoscan_enable),
+      data_ready_(false),
       event_(Message::Event::FindEvent("cmd_event")),
       cmd_tp_("cmd_ai") {
   auto rx_cplt_callback = [](void *arg) {
@@ -106,7 +107,11 @@ bool AI::Offline() {
   /* 离线移交控制权 */
   if (bsp_time_get_ms() - this->last_online_time_ > 200) {
     memset(&this->cmd_, 0, sizeof(cmd_));
-    this->cmd_.online = false;
+    if (!this->autoscan_enable_) {
+      this->cmd_.online = true;
+    } else {
+      this->cmd_.online = false;
+    }
     this->cmd_.ctrl_source = Component::CMD::CTRL_SOURCE_AI;
     this->cmd_tp_.Publish(this->cmd_);
   }
@@ -117,6 +122,7 @@ bool AI::PackMCU() {
   this->to_host_.mcu.id = AI_ID_MCU;
   memcpy(&(this->to_host_.mcu.package.data.quat), &(this->quat_),
          sizeof(this->quat_));
+  this->to_host_.mcu.package.data.notice = this->notice_for_ai_;
   this->to_host_.mcu.package.crc16 = Component::CRC16::Calculate(
       reinterpret_cast<const uint8_t *>(&(this->to_host_.mcu.package)),
       sizeof(this->to_host_.mcu.package) - sizeof(uint16_t), CRC16_INIT);
@@ -140,25 +146,27 @@ bool AI::PackRef() {
   return true;
 }
 
-/* 后续通信协议要改：
- 1、电控say导航到哪里
- 2、视觉say是否是导航状态（电控要求一个位置，如果导航到或者已经在此为止，则算法反馈一个“已完成”，表示到达目的地附近）
- 3、建议notice分开成
- 底盘、云台、发射三个notice，方便三路分开也避免错误置零的情况
-
-底盘：电控->视觉123
-云台：视觉->电控 是否看见
-发射：视觉->电控 能否发弹
-
- */
 void AI::DecideAction() {
+  memcpy(&(this->cmd_.gimbal.eulr), &(this->from_host_.data.gimbal),
+         sizeof(this->cmd_.gimbal.eulr));
   this->notice_ = this->from_host_.data.notice;
+  /* AI自瞄数据不更新，重置notice_ */
+  if (this->cmd_.gimbal.eulr.yaw == this->last_eulr_.yaw ||
+      this->cmd_.gimbal.eulr.pit == this->last_eulr_.pit) {
+    this->notice_ = 0;
+  }
+  memcpy(&(this->last_eulr_), &(this->cmd_.gimbal.eulr),
+         sizeof(this->cmd_.gimbal.eulr));
 
   /* 判定是否受击打 */
   if (this->raw_ref_.robot_damage.damage_type == 0x0 && !last_damage_type_) {
     this->is_damaged_ = true;
     this->damaged_id_ = this->raw_ref_.robot_damage.armor_id;
     this->damaged_time_ = bsp_time_get_ms();
+    this->damaged_yaw_ = this->gimbal_data_.yaw_for_chassis;
+  }
+  if (bsp_time_get_ms() - this->damaged_time_ > 1000) {
+    is_damaged_ = false;
   }
   this->last_damage_type_ = this->raw_ref_.robot_damage.damage_type;
 
@@ -173,11 +181,14 @@ void AI::DecideAction() {
   if (can_start_navigation_) {
     if (this->ref_.outpost_hp > 50) { /* 认为哨兵&基地无敌 */
       this->action_.ai_chassis = AI::Action::TO_OUTPOST;
+      this->notice_for_ai_ = AI::Action::TO_OUTPOST;
     } else {
       if (this->ref_.hp < 50) { /* max = 400 */
         this->action_.ai_chassis = AI::Action::TO_SUPPLY;
+        this->notice_for_ai_ = AI::Action::TO_SUPPLY;
       } else {
         this->action_.ai_chassis = AI::Action::TO_PATROL_AREA;
+        this->notice_for_ai_ = AI::Action::TO_PATROL_AREA;
       }
 
       //   this->action_.ai_chassis = AI::Action::TO_HIGHLAND;
@@ -186,17 +197,17 @@ void AI::DecideAction() {
   } else { /* 导航已完成/受打击 */
     this->action_.ai_chassis = AI::Action::ROTOR;
   }
+  this->action_.ai_chassis =
+      AI::Action::TO_OUTPOST;  //导航测试用，记得删除！！！！
 
   /* 云台行为 */
-  if (this->notice_ == AI_NOTICE_AUTO_AIM) { /* 自瞄 */
+  if (this->notice_ == AI_NOTICE_AUTO_AIM ||
+      this->notice_ == AI_NOTICE_FIRE) { /* 自瞄 */
     this->action_.ai_gimbal = AI::Action::AUTO_AIM;
     this->cmd_.gimbal.mode = Component::CMD::GIMBAL_ABSOLUTE_CTRL;
   } else if (is_damaged_) { /* 反击 */
     this->cmd_.gimbal.mode = Component::CMD::GIMBAL_ABSOLUTE_CTRL;
     this->action_.ai_gimbal = AI::Action::AFFECTED;
-    if (bsp_time_get_ms() - this->damaged_time_ > 1000) {
-      is_damaged_ = false;
-    }
   } else { /* 扫描 */
     this->cmd_.gimbal.mode = Component::CMD::GIMBAL_ABSOLUTE_CTRL;
     this->action_.ai_gimbal = AI::Action::SCANF;
@@ -205,7 +216,6 @@ void AI::DecideAction() {
   /* 发射机构行为 */
   if (this->notice_ == AI_NOTICE_FIRE) { /* 开火 */
     this->action_.ai_launcher = AI::Action::FIRE;
-    this->notice_ = 0;
   } else {
     this->action_.ai_launcher = AI::Action::CEASEFIRE; /* 不发弹 */
   }
@@ -224,100 +234,106 @@ bool AI::PackCMD() {
   /* 根据模式，来发送不同的数据 */
   /* 发布控制命令 -> cmd结构体 */
 
+  /* 确保遥控器开关最高控制权，关遥控器即断控 */
   if (!Component::CMD::Online()) {
-    return false; /* 如果遥控器不在线 */
+    return false;
   }
-  memcpy(&(this->cmd_.ext.extern_channel),
-         &(this->from_host_.data.extern_channel),
-         sizeof(this->cmd_.ext.extern_channel));
 
   /* 控制权在AI */
   if (Component::CMD::GetCtrlSource() == Component::CMD::CTRL_SOURCE_AI) {
+    /* AUTO控制模式，用于全自动机器人 */
     if (Component::CMD::GetCtrlMode() == Component::CMD::CMD_AUTO_CTRL) {
-      if (this->cmd_.online) { /* AI在线 */
-        switch (this->action_.ai_chassis) {
-          case TO_BASE:
-          case TO_OUTPOST:
-          case TO_SUPPLY:
-          case TO_HIGHLAND:
-          case TO_PATROL_AREA:
-            memcpy(&(this->cmd_.chassis),
-                   &(this->from_host_.data.chassis_move_vec),
-                   sizeof(this->from_host_.data.chassis_move_vec));
-            break;
-          case ROTOR:
-            this->cmd_.chassis.x = 0;
-            this->cmd_.chassis.y = 0;
-            this->cmd_.chassis.z = 0.5;  //待修改
-            break;
-        }
-
-        switch (this->action_.ai_gimbal) {
-          case AUTO_AIM:
-            memcpy(&(this->cmd_.gimbal.eulr), &(this->from_host_.data.gimbal),
-                   sizeof(this->cmd_.gimbal.eulr));
-            break;
-          case AFFECTED:
-            this->cmd_.gimbal.eulr.yaw = this->damaged_id_ * (M_2PI / 4.0);
-            this->cmd_.gimbal.eulr.pit = 4.5f;  //待修改成合适角度
-            this->last_yaw_angle_ = this->gimbal_data_.get_yaw_angle - 1.58;
-            this->target_angle_ = 0.0;
-
-            break;
-          case SCANF:
-            // this->target_angle_ +=
-            // this->gimbal_data_.scanf_mode.scanf_yaw_rate;
-            // this->cmd_.gimbal.eulr.yaw =
-            //     this->last_yaw_angle_ + this->target_angle_;
-
-            // this->cmd_.gimbal.eulr.pit =
-            //     this->gimbal_data_.scanf_mode.scanf_pit_center +
-            //     this->gimbal_data_.scanf_mode.scanf_pit_range *
-            //         sin(this->gimbal_data_.scanf_mode.scanf_pit_omega *
-            //             bsp_time_get_ms() / 1000.0);
-            this->cmd_.gimbal.eulr.pit = 4.5;
-            this->cmd_.gimbal.eulr.yaw = 0;
-            break;
-        }
-
-        switch (this->action_.ai_launcher) {
-          case FIRE:
-            this->event_.Active(AI_FIRE_COMMAND);  //写热量控制！！最大化利用
-          case CEASEFIRE:
-            break;
-        }
-
-        switch (this->action_.ai_to_referee) {
-          case CONFIRM_RESURRECTION:
-            cmd_for_ref_.confirm_resurrection = 1;
-            cmd_for_ref_.buy_resurrection = 1;
-            cmd_for_ref_.buy_bullet_num = 0;
-            cmd_for_ref_.remote_buy_bullet_times = 0;
-            cmd_for_ref_.romote_buy_hp_times = 0;
-            break;
-          case EXCHANGE_BULLETS:
-            // cmd_for_ref_.confirm_resurrection = 0;
-            // cmd_for_ref_.buy_resurrection = 0;
-            // cmd_for_ref_.buy_bullet_num = this->last_buy_bullet_num_ + 50;
-            // cmd_for_ref_.remote_buy_bullet_times = 0;
-            // cmd_for_ref_.romote_buy_hp_times = 0;
-            break;
-          case NOTHING:
-            cmd_for_ref_.confirm_resurrection = 0;
-            cmd_for_ref_.buy_resurrection = 0;
-            cmd_for_ref_.buy_bullet_num = this->last_buy_bullet_num_;
-            cmd_for_ref_.remote_buy_bullet_times = 2;
-            cmd_for_ref_.romote_buy_hp_times = 0;
-            break;
-        }
-        this->last_buy_bullet_num_ = cmd_for_ref_.buy_bullet_num;
-      } else {
-        this->event_.Active(AI_OFFLINE);
+      switch (this->action_.ai_chassis) {
+        case TO_BASE:
+        case TO_OUTPOST:
+        case TO_SUPPLY:
+        case TO_HIGHLAND:
+        case TO_PATROL_AREA:
+          // this->event_.Active(AI_INDENPENDENT);
+          memcpy(&(this->cmd_.chassis),
+                 &(this->from_host_.data.chassis_move_vec),
+                 sizeof(this->from_host_.data.chassis_move_vec));
+          break;
+        case ROTOR:
+          /* 实现导航+rotor导航量赋值给vec */
+          // this->event_.Active(AI_ROTOR);//导航调试用，记得解除
+          this->cmd_.chassis.x = 0;
+          this->cmd_.chassis.y = 0;
+          this->cmd_.chassis.z = 0.0;  //待修改
+          break;
       }
-      this->notice_ = 0;
-      this->cmd_tp_.Publish(this->cmd_);
-    }
 
+      switch (this->action_.ai_gimbal) {
+        case AUTO_AIM:
+          memcpy(&(this->cmd_.gimbal.eulr), &(this->from_host_.data.gimbal),
+                 sizeof(this->cmd_.gimbal.eulr));
+          break;
+        case AFFECTED:
+          this->cmd_.gimbal.eulr.yaw = this->damaged_id_ * (M_2PI / 4.0);
+
+          this->cmd_.gimbal.eulr.pit = 4.5f;  //待修改成合适角度
+          this->last_yaw_angle_ = this->gimbal_data_.get_yaw_angle - 1.58;
+          this->target_angle_ = 0.0;
+
+          this->cmd_.gimbal.eulr.yaw = 0.0;  ////记得删掉！！！！！！！！
+
+          break;
+        case SCANF:
+          this->target_angle_ += this->gimbal_data_.scanf_mode.scanf_yaw_rate;
+          this->cmd_.gimbal.eulr.yaw =
+              this->last_yaw_angle_ + this->target_angle_;
+
+          this->cmd_.gimbal.eulr.pit =
+              this->gimbal_data_.scanf_mode.scanf_pit_center +
+              this->gimbal_data_.scanf_mode.scanf_pit_range *
+                  sin(this->gimbal_data_.scanf_mode.scanf_pit_omega *
+                      bsp_time_get_ms() / 1000.0);
+          this->cmd_.gimbal.eulr.yaw = 0.0;  ////记得删掉！！！！！！！！
+          break;
+      }
+
+      switch (this->action_.ai_launcher) {
+        case FIRE:
+          this->event_.Active(AI_FIRE_COMMAND);  //连续发弹
+          break;
+        case CEASEFIRE:
+          this->event_.Active(AI_STOP_FIRE);
+          break;
+      }
+
+      switch (this->action_.ai_to_referee) {
+        case CONFIRM_RESURRECTION:
+          cmd_for_ref_.confirm_resurrection = 1;
+          cmd_for_ref_.buy_resurrection = 0;
+          cmd_for_ref_.buy_bullet_num = 0;
+          cmd_for_ref_.remote_buy_bullet_times = 0;
+          cmd_for_ref_.romote_buy_hp_times = 0;
+          break;
+        case EXCHANGE_BULLETS:
+          cmd_for_ref_.confirm_resurrection = 0;
+          cmd_for_ref_.buy_resurrection = 0;
+          cmd_for_ref_.buy_bullet_num = this->last_buy_bullet_num_ + 50;
+          cmd_for_ref_.remote_buy_bullet_times = 0;
+          cmd_for_ref_.romote_buy_hp_times = 0;
+          break;
+        case NOTHING:
+          cmd_for_ref_.confirm_resurrection = 0;
+          cmd_for_ref_.buy_resurrection = 0;
+          cmd_for_ref_.buy_bullet_num = this->last_buy_bullet_num_;
+          cmd_for_ref_.remote_buy_bullet_times = 2;
+          cmd_for_ref_.romote_buy_hp_times = 0;
+          break;
+      }
+      this->last_buy_bullet_num_ = cmd_for_ref_.buy_bullet_num;
+
+      /* 比赛开始前不运行 */
+      if (this->ref_.game_progress == GAMING ||
+          this->ref_.game_progress == PREPARATION ||
+          this->ref_.game_progress == WAITTING) {
+        this->cmd_tp_.Publish(this->cmd_);
+      }
+    }
+    /* OP控制模式，用于鼠标右键自瞄 */
     else if (Component::CMD::GetCtrlMode() == Component::CMD::CMD_OP_CTRL) {
       memcpy(&(this->cmd_.gimbal.eulr), &(this->from_host_.data.gimbal),
              sizeof(this->cmd_.gimbal.eulr));
@@ -329,6 +345,10 @@ bool AI::PackCMD() {
       memcpy(&(this->cmd_.chassis), &(this->from_host_.data.chassis_move_vec),
              sizeof(this->from_host_.data.chassis_move_vec));
 
+      memcpy(&(this->cmd_.ext.extern_channel),
+             &(this->from_host_.data.extern_channel),
+             sizeof(this->cmd_.ext.extern_channel));
+
       this->notice_ = this->from_host_.data.notice;
 
       this->cmd_.ctrl_source = Component::CMD::CTRL_SOURCE_AI;
diff --git a/src/device/ai/dev_ai.hpp b/src/device/ai/dev_ai.hpp
index b0e5dc20..e5cef5dd 100644
--- a/src/device/ai/dev_ai.hpp
+++ b/src/device/ai/dev_ai.hpp
@@ -53,14 +53,16 @@ class AI {
     float yaw_for_chassis;
     ScanfMode scanf_mode;
     float get_yaw_angle;
+    Component::Type::CycleValue start_imu_yaw;
   } GimbalData;
 
   typedef enum {
-    AI_OFFLINE = 128,
-    // AI_FIND_TARGET,
-    // AI_AUTOPATROL,
-    AI_TURN,
+    AI_OFFLINE = 127,
+    AI_INDENPENDENT,
+    AI_FOLLOW,
+    AI_ROTOR,
     AI_FIRE_COMMAND,
+    AI_STOP_FIRE,
   } AIControlData;
 
   typedef enum {
@@ -107,7 +109,7 @@ class AI {
     uint8_t ai_to_referee; /* 确认复活、购买发弹量、购买血量 */
   } AICtrlAction;
 
-  AI();
+  AI(bool autoscan_enable = false);
 
   bool StartRecv();
 
@@ -128,6 +130,8 @@ class AI {
   void DecideAction();
 
  private:
+  bool autoscan_enable_ = true; /* ai自动扫描，不启用则默认ai全程在线 */
+
   bool ref_updated_ = false;
 
   uint32_t last_online_time_ = 0;
@@ -149,6 +153,7 @@ class AI {
   bool is_damaged_;
   uint8_t damaged_id_;
   uint32_t damaged_time_;
+  Component::Type::CycleValue damaged_yaw_;
 
   System::Thread thread_;
 
@@ -169,6 +174,7 @@ class AI {
   float last_yaw_angle_;
 
   SentryDecisionData cmd_for_ref_;
+
   uint8_t last_buy_bullet_num_ = 0;
   Component::Type::CycleValue target_angle_ = 0.0;
 
@@ -176,6 +182,16 @@ class AI {
       Message::Topic<SentryDecisionData>("sentry_cmd_for_ref");
 
   Component::Type::Quaternion quat_{};
+  uint8_t notice_for_ai_;
+
   Device::Referee::Data raw_ref_{};
+
+  Component::Type::Eulr eulr_;
+
+  struct {
+    float yaw; /* 偏航角（Yaw angle） */
+    float pit; /* 俯仰角（Pitch angle） */
+    float rol; /* 翻滚角（Roll angle） */
+  } last_eulr_;
 };
 }  // namespace Device
diff --git a/src/device/referee/dev_referee.cpp b/src/device/referee/dev_referee.cpp
index 124c10bd..e0fef17d 100644
--- a/src/device/referee/dev_referee.cpp
+++ b/src/device/referee/dev_referee.cpp
@@ -37,6 +37,8 @@ using namespace Device;
 static uint8_t rxbuf[REF_LEN_RX_BUFF];
 
 Referee::UIPack Referee::ui_pack_;
+Referee::SentryPack Referee::sentry_pack_;
+
 Referee *Referee::self_;
 
 Referee::Referee() : event_(Message::Event::FindEvent("cmd_event")) {
@@ -97,17 +99,18 @@ Referee::Referee() : event_(Message::Event::FindEvent("cmd_event")) {
   this->recv_thread_.Create(ref_recv_thread, this, "ref_recv_thread",
                             DEVICE_REF_RECV_TASK_STACK_DEPTH,
                             System::Thread::REALTIME);
+
   auto ref_trans_thread = [](Referee *ref) {
     uint32_t last_online_time = bsp_time_get_ms();
     auto ai_sub = Message::Subscriber<SentryDecisionData>("sentry_cmd_for_ref");
     while (1) {
       ai_sub.DumpData(ref->data_from_sentry_);
       ref->UpdateUI(); /* 更新UI */
-
-      if (ref->ref_data_.robot_status.robot_id == REF_BOT_RED_SENTRY ||
-          ref->ref_data_.robot_status.robot_id == REF_BOT_BLU_SENTRY) {
-        ref->SentryDecision(); /* 发布哨兵决策 */
-      }
+                       // ref->SentryDecision(); /* 发布哨兵决策 */
+      //    if (ref->ref_data_.robot_status.robot_id == REF_BOT_RED_SENTRY ||
+      //        ref->ref_data_.robot_status.robot_id == REF_BOT_BLU_SENTRY) {
+      //      ref->SentryDecision(); /* 发布哨兵决策 */
+      //    }
 
       ref->trans_thread_.SleepUntil(40, last_online_time);
     }
@@ -391,7 +394,6 @@ bool Referee::UpdateUI() {
     done = true;
     op = Component::UI::UI_GRAPHIC_OP_REWRITE;
   }
-
   if (!done && this->string_data_.Size() > 0) {
     cmd_id = REF_STDNT_CMD_ID_UI_STR;
     pack_size = sizeof(UIStringPack);
@@ -443,7 +445,7 @@ bool Referee::UpdateUI() {
     return false;
   }
 
-  this->ui_pack_.raw.cmd_id = REF_CMD_ID_INTER_STUDENT;
+  this->ui_pack_.raw.cmd_id = REF_CMD_ID_INTER_STUDENT; /* 0x0301 */
 
   SetUIHeader(this->ui_pack_.raw.student_header, cmd_id,
               static_cast<RobotID>(this->ref_data_.robot_status.robot_id));
@@ -486,8 +488,37 @@ bool Referee::UpdateUI() {
         pack_size - sizeof(uint16_t), CRC16_INIT);
   }
 
-  bsp_uart_transmit(BSP_UART_REF, reinterpret_cast<uint8_t *>(&this->ui_pack_),
-                    pack_size, false);
+  /* sentry begin */
+  if (ref_data_.robot_status.robot_id == REF_BOT_BLU_SENTRY ||
+      ref_data_.robot_status.robot_id == REF_BOT_RED_SENTRY) {
+    SetPacketHeader(this->sentry_pack_.frame_header, pack_size);
+
+    this->sentry_pack_.cmd_id = REF_CMD_ID_INTER_STUDENT;  // 0x0301
+
+    SetSentryHeader(
+        this->sentry_pack_.student_header, REF_STDNT_CMD_ID_SENTRY_CMD,
+        static_cast<RobotID>(this->ref_data_.robot_status.robot_id));
+    //设置子ID、发送者、接受者
+
+    // this->sentry_pack_.sentry_cmd = this->data_from_sentry_;
+    this->sentry_pack_.data_cmd = 1;
+
+    this->sentry_pack_.crc16 = Component::CRC16::Calculate(
+        reinterpret_cast<const uint8_t *>(&this->sentry_pack_),
+        pack_size - sizeof(uint16_t), CRC16_INIT);
+
+    /* 开始传输!!!!!!!!!!!!!!!!!!!!!! */
+    bsp_uart_transmit(BSP_UART_REF,
+                      reinterpret_cast<uint8_t *>(&this->sentry_pack_),
+                      pack_size, false);
+  }
+
+  /* sentry end */
+  else {
+    bsp_uart_transmit(BSP_UART_REF,
+                      reinterpret_cast<uint8_t *>(&this->ui_pack_), pack_size,
+                      false);
+  }
 
   this->ui_lock_.Post();
 
@@ -496,11 +527,19 @@ bool Referee::UpdateUI() {
 
 bool Referee::SentryDecision() {
   uint32_t pack_size = 0;
-  pack_size = sizeof(InterStudentHeader) + sizeof(SentryDecisionData);
-  this->packet_sent_.Wait(UINT32_MAX);  //上锁
+  bool done = false;
+
+  this->packet_sent_.Wait(200);  //上锁
 
   this->sentry_lock_.Wait(UINT32_MAX);  //上锁
 
+  // if (!done && this->sentry_data_.Size() > 0) {  //队列del
+  //   pack_size = sizeof(InterStudentHeader) + sizeof(SentryDecisionData);
+  //   done = true;
+  // }
+
+  // sentry_data_.Reset();  //队列重置ele
+
   SetPacketHeader(this->sentry_pack_.frame_header, pack_size);
 
   this->sentry_pack_.cmd_id = REF_CMD_ID_INTER_STUDENT;  // 0x0301
@@ -510,15 +549,22 @@ bool Referee::SentryDecision() {
                   static_cast<RobotID>(this->ref_data_.robot_status.robot_id));
   //设置子ID、发送者、接受者
 
-  this->sentry_pack_.sentry_cmd = this->data_from_sentry_;
+  // this->sentry_pack_.sentry_cmd = this->data_from_sentry_;
 
   this->sentry_pack_.crc16 = Component::CRC16::Calculate(
       reinterpret_cast<const uint8_t *>(&this->sentry_pack_),
       pack_size - sizeof(uint16_t), CRC16_INIT);
 
+  // this->sentry_data_.Receive(this->sentry_pack_.sentry_cmd);  //队列 接收
+
   bsp_uart_transmit(BSP_UART_REF,
                     reinterpret_cast<uint8_t *>(&this->sentry_pack_), pack_size,
                     false);
+  if (!done) {
+    this->sentry_lock_.Post();
+    this->packet_sent_.Post();
+    return false;
+  }
 
   this->sentry_lock_.Post();
   return true;
@@ -571,7 +617,11 @@ void Referee::SetSentryHeader(Referee::InterStudentHeader &header,
                               const Referee::CMDID CMD_ID,
                               Referee::RobotID robot_id) {
   header.cmd_id = CMD_ID;
-  header.id_sender = robot_id;
+  if (robot_id > 100) {
+    header.id_sender = 0x0107;
+  } else {
+    header.id_sender = 0x0007;
+  }
   header.id_receiver = 0x8080;
 }
 
diff --git a/src/device/referee/dev_referee.hpp b/src/device/referee/dev_referee.hpp
index b5042c00..2120f434 100644
--- a/src/device/referee/dev_referee.hpp
+++ b/src/device/referee/dev_referee.hpp
@@ -508,10 +508,9 @@ class Referee {
   typedef struct __attribute__((packed)) {
     Header frame_header;  // 0x0301
     uint16_t cmd_id;
-
-    Referee::InterStudentHeader student_header;  //含0x0102
-    SentryDecisionData sentry_cmd;
-
+    Referee::InterStudentHeader student_header;  //含0x0120
+    // SentryDecisionData sentry_cmd;
+    uint32_t data_cmd;
     uint16_t crc16;
   } SentryPack;
 
@@ -582,6 +581,9 @@ class Referee {
   System::Queue<Component::UI::Del> static_del_data_ =
       System::Queue<Component::UI::Del>(10);
 
+  System::Queue<SentryDecisionData> sentry_data_ =
+      System::Queue<SentryDecisionData>(10);
+
   System::Semaphore ui_lock_ = System::Semaphore(true);
 
   System::Semaphore sentry_lock_ = System::Semaphore(true);
@@ -594,7 +596,7 @@ class Referee {
 
   static UIPack ui_pack_;
 
-  SentryPack sentry_pack_;
+  static SentryPack sentry_pack_;
 
   static Referee *self_;
   SentryDecisionData data_from_sentry_;
diff --git a/src/module/gimbal/mod_gimbal.cpp b/src/module/gimbal/mod_gimbal.cpp
index 3be23c90..5ea2f8b3 100644
--- a/src/module/gimbal/mod_gimbal.cpp
+++ b/src/module/gimbal/mod_gimbal.cpp
@@ -175,7 +175,6 @@ void Gimbal::SetMode(Mode mode) {
       this->setpoint_.eulr_.yaw = this->eulr_.yaw;
     }
   }
-
   this->mode_ = mode;
 }
 
diff --git a/src/module/launcher/mod_launcher.cpp b/src/module/launcher/mod_launcher.cpp
index 961485ce..838cb9ab 100644
--- a/src/module/launcher/mod_launcher.cpp
+++ b/src/module/launcher/mod_launcher.cpp
@@ -31,9 +31,13 @@ Launcher::Launcher(Param& param, float control_freq)
     launcher->ctrl_lock_.Wait(UINT32_MAX);
     switch (event) { /* 根据event设置模式 */
       case CHANGE_FIRE_MODE_RELAX:
+        launcher->SetFireMode(static_cast<FireMode>(RELAX));
+        break;
       case CHANGE_FIRE_MODE_SAFE:
+        launcher->SetFireMode(static_cast<FireMode>(SAFE));
+        break;
       case CHANGE_FIRE_MODE_LOADED:
-        launcher->SetFireMode(static_cast<FireMode>(event));
+        launcher->SetFireMode(static_cast<FireMode>(LOADED));
         break;
       case LAUNCHER_START_FIRE: /* 摩擦轮开启条件下，开火控制fire为ture */
         if (launcher->fire_ctrl_.fire_mode_ == LOADED) {
@@ -51,12 +55,17 @@ Launcher::Launcher(Param& param, float control_freq)
         launcher->SetTrigMode(static_cast<TrigMode>(
             (launcher->fire_ctrl_.trig_mode_ + 1) % CONTINUED));
         break;
+      case LAUNCHER_STOP_TRIG:
+        launcher->SetTrigMode(static_cast<TrigMode>(STOP));
+        break;
+
       case OPEN_COVER:
         launcher->cover_mode_ = OPEN;
         break;
       case CLOSE_COVER:
         launcher->cover_mode_ = CLOSE;
         break;
+
       default:
         break;
     }
@@ -133,6 +142,9 @@ void Launcher::Control() {
     case BURST: /* 爆发开火模式 */
       max_burst = 5;
       break;
+    case STOP:
+      max_burst = 0;
+      break;
     default:
       max_burst = 1;
       break;
@@ -141,6 +153,7 @@ void Launcher::Control() {
   switch (this->fire_ctrl_.trig_mode_) {
     case SINGLE: /* 点射开火模式 */
     case BURST:  /* 爆发开火模式 */
+    case STOP:
 
       /* 计算是否是第一次按下开火键 */
       this->fire_ctrl_.first_pressed_fire =
@@ -168,10 +181,13 @@ void Launcher::Control() {
     case CONTINUED: { /* 持续开火模式 */
       float launch_freq = this->LimitLauncherFreq();
       this->fire_ctrl_.launch_delay =
-          (launch_freq == 0.0f) ? UINT32_MAX
-                                : static_cast<uint32_t>(1000.f / launch_freq);
+          (launch_freq == 0.0f)
+              ? UINT32_MAX
+              : static_cast<uint32_t>(1000.f / launch_freq); /* 毫秒级延时 */
+
       break;
     }
+
     default:
       break;
   }
@@ -198,6 +214,7 @@ void Launcher::Control() {
     if ((fire_ctrl_.last_trig_angle - trig_angle_) / M_2PI *
             this->param_.num_trig_tooth >
         0.9) {
+      /* 判定为未卡弹 */
       if (!fire_ctrl_.stall) {
         fire_ctrl_.last_trig_angle = this->setpoint_.trig_angle_;
         /* 将拨弹电机角度进行循环加法，每次加(减)射出一颗弹丸的弧度变化 */
@@ -306,8 +323,7 @@ void Launcher::HeatLimit() {
           this->ref_.robot_status.shooter_cooling_value;
       this->heat_ctrl_.heat_increase = GAME_HEAT_INCREASE_17MM;
     }
-    /* 检测热量更新后,计算可发射弹丸 */
-
+    /* 检测热量更新后,计算可发射弹丸（裁判系统数据更新有延迟） */
     if ((this->heat_ctrl_.heat != this->heat_ctrl_.last_heat) ||
         this->heat_ctrl_.available_shot == 0 || (this->heat_ctrl_.heat == 0)) {
       this->heat_ctrl_.available_shot = static_cast<uint32_t>(
@@ -335,8 +351,8 @@ void Launcher::PraseRef() {
 
 float Launcher::LimitLauncherFreq() { /* 热量限制计算 */
   float heat_percent = this->heat_ctrl_.heat / this->heat_ctrl_.heat_limit;
-  float stable_freq =
-      this->heat_ctrl_.cooling_rate / this->heat_ctrl_.heat_increase;
+  float stable_freq = this->heat_ctrl_.cooling_rate /
+                      this->heat_ctrl_.heat_increase; /* 每秒可发弹量 */
   if (this->param_.model == LAUNCHER_MODEL_42MM) {
     return stable_freq;
   } else {
diff --git a/src/module/launcher/mod_launcher.hpp b/src/module/launcher/mod_launcher.hpp
index b7e0c773..05808e9a 100644
--- a/src/module/launcher/mod_launcher.hpp
+++ b/src/module/launcher/mod_launcher.hpp
@@ -24,6 +24,7 @@ class Launcher {
     SINGLE,    /* 单发开火模式  */
     BURST,     /* N爆发开火模式 */
     CONTINUED, /* 持续开火模式 */
+    STOP,
   } TrigMode;
 
   typedef enum { OPEN, CLOSE } CoverMode;
@@ -32,12 +33,15 @@ class Launcher {
     CHANGE_FIRE_MODE_RELAX,
     CHANGE_FIRE_MODE_SAFE,
     CHANGE_FIRE_MODE_LOADED,
+    LAUNCHER_START_FIRE, /* 开火，拨弹盘开始发弹 */
+
     CHANGE_TRIG_MODE_SINGLE,
     CHANGE_TRIG_MODE_BURST,
     CHANGE_TRIG_MODE,
+    LAUNCHER_STOP_TRIG,
+
     OPEN_COVER,
     CLOSE_COVER,
-    LAUNCHER_START_FIRE, /* 开火，拨弹盘开始发弹 */
   } LauncherEvent;
 
   enum {
diff --git a/src/robot/sentry/robot.cpp b/src/robot/sentry/robot.cpp
index 5fc98f80..1589928e 100644
--- a/src/robot/sentry/robot.cpp
+++ b/src/robot/sentry/robot.cpp
@@ -38,10 +38,18 @@ Robot::Sentry::Param param = {
       Component::CMD::EventMapItem{
         Device::DR16::DR16_SW_L_POS_BOT,
         Module::RMChassis::SET_MODE_ROTOR
+      },
+       Component::CMD::EventMapItem{
+        Device::AI::AI_INDENPENDENT,
+        Module::RMChassis::SET_MODE_INDENPENDENT
+      },
+       Component::CMD::EventMapItem{
+        Device::AI::AI_FOLLOW,
+        Module::RMChassis::SET_MODE_FOLLOW
       },
       Component::CMD::EventMapItem{
-        Device::AI::AI_OFFLINE,
-        Module::RMChassis::SET_MODE_RELAX
+        Device::AI::AI_ROTOR,
+        Module::RMChassis::SET_MODE_ROTOR
       }
     },
 
@@ -163,7 +171,7 @@ Robot::Sentry::Param param = {
     .yaw_actr = {
       .speed = {
           /* GIMBAL_CTRL_YAW_OMEGA_IDX */
-          .k = 0.28f,
+          .k = 0.20f,
           .p = 1.f,
           .i = 1.f,
           .d = 0.f,
@@ -391,13 +399,21 @@ Robot::Sentry::Param param = {
         Device::DR16::DR16_SW_R_POS_BOT,/* 模拟未找到目标，巡逻模式 */
         Module::Launcher::LAUNCHER_START_FIRE
       },
-        Component::CMD::EventMapItem{
-        Device::AI::AIControlData::AI_FIRE_COMMAND,
-        Module::Launcher::LAUNCHER_START_FIRE
+       Component::CMD::EventMapItem{
+        Device::DR16::DR16_SW_R_POS_BOT,/* 模拟未找到目标，巡逻模式 */
+        Module::Launcher::CHANGE_TRIG_MODE_BURST
       },
+      //   Component::CMD::EventMapItem{
+      //   Device::AI::AIControlData::AI_FIRE_COMMAND,
+      //   Module::Launcher::LAUNCHER_START_FIRE
+      // },
       Component::CMD::EventMapItem{
         Device::AI::AIControlData::AI_FIRE_COMMAND,
-        Module::Launcher::CHANGE_TRIG_MODE_SINGLE
+        Module::Launcher::CHANGE_TRIG_MODE_BURST
+      },
+      Component::CMD::EventMapItem{
+        Device::AI::AIControlData::AI_STOP_FIRE,
+        Module::Launcher::LAUNCHER_STOP_TRIG
       }
 
     },
@@ -505,7 +521,7 @@ Robot::Sentry::Param param = {
     },
 
     .EVENT_MAP = {
-     Component::CMD::EventMapItem{
+      Component::CMD::EventMapItem{
         Component::CMD::CMD_EVENT_LOST_CTRL,
         Module::Launcher::CHANGE_FIRE_MODE_RELAX
       },
@@ -525,13 +541,22 @@ Robot::Sentry::Param param = {
         Device::DR16::DR16_SW_R_POS_BOT,/* 模拟未找到目标，巡逻模式 */
         Module::Launcher::LAUNCHER_START_FIRE
       },
-        Component::CMD::EventMapItem{
-        Device::AI::AIControlData::AI_FIRE_COMMAND,
-        Module::Launcher::LAUNCHER_START_FIRE
+       Component::CMD::EventMapItem{
+        Device::DR16::DR16_SW_R_POS_BOT,/* 模拟未找到目标，巡逻模式 */
+        Module::Launcher::CHANGE_TRIG_MODE_BURST
       },
+
+      //   Component::CMD::EventMapItem{
+      //   Device::AI::AIControlData::AI_FIRE_COMMAND,
+      //   Module::Launcher::LAUNCHER_START_FIRE
+      // },
       Component::CMD::EventMapItem{
         Device::AI::AIControlData::AI_FIRE_COMMAND,
-        Module::Launcher::CHANGE_TRIG_MODE_SINGLE
+        Module::Launcher::CHANGE_TRIG_MODE_BURST
+      },
+      Component::CMD::EventMapItem{
+        Device::AI::AIControlData::AI_STOP_FIRE,
+        Module::Launcher::LAUNCHER_STOP_TRIG
       }
 
     },
diff --git a/src/robot/sentry/robot.hpp b/src/robot/sentry/robot.hpp
index 29b5d8a6..95e1b8cb 100644
--- a/src/robot/sentry/robot.hpp
+++ b/src/robot/sentry/robot.hpp
@@ -27,6 +27,7 @@ class Sentry {
   Component::CMD cmd_;
 
   Device::AI ai_;
+
   Device::AHRS ahrs_;
   Device::BMI088 bmi088_;
   Device::Can can_;
@@ -41,6 +42,7 @@ class Sentry {
   Module::Launcher launcher2_;
   Sentry(Param& param, float control_freq)
       : cmd_(Component::CMD::CMD_AUTO_CTRL),
+        ai_(false),
         bmi088_(param.bmi088_rot),
         cap_(param.cap),
         chassis_(param.chassis, control_freq),
-- 
Gitee


From f2540cfec4b5e33a4f7e5b7795f27591ba20ceb3 Mon Sep 17 00:00:00 2001
From: Marine_zxy <1423266892@qq.com>
Date: Mon, 13 May 2024 22:15:01 +0800
Subject: [PATCH 15/19] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E8=A7=86=E8=A7=89?=
 =?UTF-8?q?=E6=9A=82=E7=95=99?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/device/ai/dev_ai.cpp | 6 ++++--
 src/device/ai/dev_ai.hpp | 1 +
 2 files changed, 5 insertions(+), 2 deletions(-)

diff --git a/src/device/ai/dev_ai.cpp b/src/device/ai/dev_ai.cpp
index dce01468..58a207d1 100644
--- a/src/device/ai/dev_ai.cpp
+++ b/src/device/ai/dev_ai.cpp
@@ -154,6 +154,8 @@ void AI::DecideAction() {
   if (this->cmd_.gimbal.eulr.yaw == this->last_eulr_.yaw ||
       this->cmd_.gimbal.eulr.pit == this->last_eulr_.pit) {
     this->notice_ = 0;
+  } else {
+    this->aim_time_ = bsp_time_get_ms(); /* 自瞄锁定时刻 */
   }
   memcpy(&(this->last_eulr_), &(this->cmd_.gimbal.eulr),
          sizeof(this->cmd_.gimbal.eulr));
@@ -201,8 +203,8 @@ void AI::DecideAction() {
       AI::Action::TO_OUTPOST;  //导航测试用，记得删除！！！！
 
   /* 云台行为 */
-  if (this->notice_ == AI_NOTICE_AUTO_AIM ||
-      this->notice_ == AI_NOTICE_FIRE) { /* 自瞄 */
+  if (this->notice_ == AI_NOTICE_AUTO_AIM || this->notice_ == AI_NOTICE_FIRE ||
+      bsp_time_get_ms() - this->aim_time_ < 1000) { /* 自瞄 */
     this->action_.ai_gimbal = AI::Action::AUTO_AIM;
     this->cmd_.gimbal.mode = Component::CMD::GIMBAL_ABSOLUTE_CTRL;
   } else if (is_damaged_) { /* 反击 */
diff --git a/src/device/ai/dev_ai.hpp b/src/device/ai/dev_ai.hpp
index e5cef5dd..93de344b 100644
--- a/src/device/ai/dev_ai.hpp
+++ b/src/device/ai/dev_ai.hpp
@@ -155,6 +155,7 @@ class AI {
   uint32_t damaged_time_;
   Component::Type::CycleValue damaged_yaw_;
 
+  uint32_t aim_time_;
   System::Thread thread_;
 
   System::Semaphore data_ready_;
-- 
Gitee


From abc03e07554a030097d012582a64afed9f439065 Mon Sep 17 00:00:00 2001
From: Marine_zxy <1423266892@qq.com>
Date: Wed, 15 May 2024 22:48:07 +0800
Subject: [PATCH 16/19] =?UTF-8?q?=E5=93=A8=E5=85=B5=E8=A1=8C=E4=B8=BA?=
 =?UTF-8?q?=E5=86=B3=E7=AD=96=E5=AE=8C=E5=96=84?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/device/ai/dev_ai.cpp | 96 ++++++++++++++++++----------------------
 src/device/ai/dev_ai.hpp | 39 ++++++++--------
 2 files changed, 63 insertions(+), 72 deletions(-)

diff --git a/src/device/ai/dev_ai.cpp b/src/device/ai/dev_ai.cpp
index 58a207d1..c5232816 100644
--- a/src/device/ai/dev_ai.cpp
+++ b/src/device/ai/dev_ai.cpp
@@ -37,9 +37,12 @@ AI::AI(bool autoscan_enable)
     auto ref_sub = Message::Subscriber<Device::Referee::Data>("referee");
     auto gimbal_sub = Message::Subscriber<GimbalData>("gimbal_data");
 
+    auto eulr_sub = Message::Subscriber<Component::Type::Eulr>("imu_eulr");
+
     while (1) {
       /* 接收云台数据 */
       gimbal_sub.DumpData(ai->gimbal_data_);
+      eulr_sub.DumpData(ai->eulr_); /* imu */
 
       /* 接收裁判系统数据 */
       if (ref_sub.DumpData(ai->raw_ref_)) {
@@ -160,57 +163,66 @@ void AI::DecideAction() {
   memcpy(&(this->last_eulr_), &(this->cmd_.gimbal.eulr),
          sizeof(this->cmd_.gimbal.eulr));
 
-  /* 判定是否受击打 */
-  if (this->raw_ref_.robot_damage.damage_type == 0x0 && !last_damage_type_) {
+  /* 判定是否受击打，一定时间内第一次受击打方向优先级最高 */
+  if (this->raw_ref_.robot_damage.damage_type == 0x0 &&
+      (this->raw_ref_.robot_damage.damage_type != last_damage_type_) &&
+      (is_damaged_ == false)) {
     this->is_damaged_ = true;
     this->damaged_id_ = this->raw_ref_.robot_damage.armor_id;
+    this->imu_yaw_now_ = this->eulr_.yaw;
+    this->damaged_yaw_ = this->gimbal_data_.yaw_;
     this->damaged_time_ = bsp_time_get_ms();
-    this->damaged_yaw_ = this->gimbal_data_.yaw_for_chassis;
   }
-  if (bsp_time_get_ms() - this->damaged_time_ > 1000) {
+  if (bsp_time_get_ms() - this->damaged_time_ > 1500) {
     is_damaged_ = false;
   }
   this->last_damage_type_ = this->raw_ref_.robot_damage.damage_type;
 
   /* 判定是否可以开始导航 */
-  if (/* 到达目的地附近|| */ is_damaged_) {
+  if (this->from_host_.data.chassis_move_vec.vx == 0 && is_damaged_) {
     can_start_navigation_ = false;
   } else {
     can_start_navigation_ = true;
   }
 
+  /* 裁判系统行为：确认复活、购买发弹量 */
+  if (this->ref_.hp == 0) {
+    this->action_.ai_to_referee = CONFIRM_RESURRECTION;
+  } else if (this->ref_.bullet_num == 0 && this->ref_.coin_num > 200) {
+    this->action_.ai_to_referee = EXCHANGE_BULLETS;
+  } else {
+    this->action_.ai_to_referee = NOTHING;
+  }
+
   /* 底盘行为*/
   if (can_start_navigation_) {
-    if (this->ref_.outpost_hp > 50) { /* 认为哨兵&基地无敌 */
-      this->action_.ai_chassis = AI::Action::TO_OUTPOST;
-      this->notice_for_ai_ = AI::Action::TO_OUTPOST;
+    if (this->ref_.outpost_hp > 200) { /* 认为哨兵&基地无敌 */
+      this->action_.ai_chassis = AI::Action::TO_HIGHWAY;
+      this->notice_for_ai_ = AI::Action::TO_HIGHWAY;
     } else {
-      if (this->ref_.hp < 50) { /* max = 400 */
+      if (this->ref_.hp < 100 ||
+          (this->action_.ai_to_referee == EXCHANGE_BULLETS)) { /* max = 400 */
         this->action_.ai_chassis = AI::Action::TO_SUPPLY;
         this->notice_for_ai_ = AI::Action::TO_SUPPLY;
       } else {
         this->action_.ai_chassis = AI::Action::TO_PATROL_AREA;
         this->notice_for_ai_ = AI::Action::TO_PATROL_AREA;
       }
-
-      //   this->action_.ai_chassis = AI::Action::TO_HIGHLAND;
-      //   this->action_.ai_chassis = AI::Action::TO_BASE;
     }
-  } else { /* 导航已完成/受打击 */
+  } else {
     this->action_.ai_chassis = AI::Action::ROTOR;
   }
-  this->action_.ai_chassis =
-      AI::Action::TO_OUTPOST;  //导航测试用，记得删除！！！！
 
   /* 云台行为 */
   if (this->notice_ == AI_NOTICE_AUTO_AIM || this->notice_ == AI_NOTICE_FIRE ||
-      bsp_time_get_ms() - this->aim_time_ < 1000) { /* 自瞄 */
+      bsp_time_get_ms() - this->aim_time_ < 1000) {
+    /* 自瞄/丢失目标1000ms内，进行视觉暂留 */
     this->action_.ai_gimbal = AI::Action::AUTO_AIM;
     this->cmd_.gimbal.mode = Component::CMD::GIMBAL_ABSOLUTE_CTRL;
-  } else if (is_damaged_) { /* 反击 */
+  } else if (is_damaged_) {
     this->cmd_.gimbal.mode = Component::CMD::GIMBAL_ABSOLUTE_CTRL;
     this->action_.ai_gimbal = AI::Action::AFFECTED;
-  } else { /* 扫描 */
+  } else {
     this->cmd_.gimbal.mode = Component::CMD::GIMBAL_ABSOLUTE_CTRL;
     this->action_.ai_gimbal = AI::Action::SCANF;
   }
@@ -221,47 +233,29 @@ void AI::DecideAction() {
   } else {
     this->action_.ai_launcher = AI::Action::CEASEFIRE; /* 不发弹 */
   }
-
-  /* 裁判系统：确认复活、购买发弹量 */
-  if (this->ref_.hp == 0) {
-    this->action_.ai_to_referee = CONFIRM_RESURRECTION;
-  } else if (this->ref_.bullet_num == 0 && this->ref_.coin_num > 50) {
-    this->action_.ai_to_referee = EXCHANGE_BULLETS;
-  } else {
-    this->action_.ai_to_referee = NOTHING;
-  }
 }
 
 bool AI::PackCMD() {
-  /* 根据模式，来发送不同的数据 */
-  /* 发布控制命令 -> cmd结构体 */
-
   /* 确保遥控器开关最高控制权，关遥控器即断控 */
   if (!Component::CMD::Online()) {
     return false;
   }
 
-  /* 控制权在AI */
+  /* 控制源：AI */
   if (Component::CMD::GetCtrlSource() == Component::CMD::CTRL_SOURCE_AI) {
     /* AUTO控制模式，用于全自动机器人 */
     if (Component::CMD::GetCtrlMode() == Component::CMD::CMD_AUTO_CTRL) {
       switch (this->action_.ai_chassis) {
-        case TO_BASE:
+        case TO_PATROL_AREA:
+        case TO_HIGHWAY:
         case TO_OUTPOST:
         case TO_SUPPLY:
-        case TO_HIGHLAND:
-        case TO_PATROL_AREA:
-          // this->event_.Active(AI_INDENPENDENT);
           memcpy(&(this->cmd_.chassis),
                  &(this->from_host_.data.chassis_move_vec),
                  sizeof(this->from_host_.data.chassis_move_vec));
           break;
         case ROTOR:
-          /* 实现导航+rotor导航量赋值给vec */
-          // this->event_.Active(AI_ROTOR);//导航调试用，记得解除
-          this->cmd_.chassis.x = 0;
-          this->cmd_.chassis.y = 0;
-          this->cmd_.chassis.z = 0.0;  //待修改
+          this->event_.Active(AI_ROTOR);
           break;
       }
 
@@ -271,32 +265,28 @@ bool AI::PackCMD() {
                  sizeof(this->cmd_.gimbal.eulr));
           break;
         case AFFECTED:
-          this->cmd_.gimbal.eulr.yaw = this->damaged_id_ * (M_2PI / 4.0);
-
-          this->cmd_.gimbal.eulr.pit = 4.5f;  //待修改成合适角度
-          this->last_yaw_angle_ = this->gimbal_data_.get_yaw_angle - 1.58;
-          this->target_angle_ = 0.0;
-
-          this->cmd_.gimbal.eulr.yaw = 0.0;  ////记得删掉！！！！！！！！
-
+          this->cmd_.gimbal.eulr.yaw = this->imu_yaw_now_ +
+                                       this->damaged_id_ * (M_2PI / 4.0) -
+                                       this->damaged_yaw_;
+          this->cmd_.gimbal.eulr.pit = this->gimbal_data_.mech_zero.pit;
+          this->last_yaw_angle_ = this->eulr_.yaw; /* 更新yaw扫描的起始位置 */
+          this->target_angle_ = 0.0; /* 置零yaw扫描的位置增量 */
           break;
         case SCANF:
           this->target_angle_ += this->gimbal_data_.scanf_mode.scanf_yaw_rate;
           this->cmd_.gimbal.eulr.yaw =
-              this->last_yaw_angle_ + this->target_angle_;
-
+              this->target_angle_ + this->last_yaw_angle_;
           this->cmd_.gimbal.eulr.pit =
               this->gimbal_data_.scanf_mode.scanf_pit_center +
               this->gimbal_data_.scanf_mode.scanf_pit_range *
                   sin(this->gimbal_data_.scanf_mode.scanf_pit_omega *
                       bsp_time_get_ms() / 1000.0);
-          this->cmd_.gimbal.eulr.yaw = 0.0;  ////记得删掉！！！！！！！！
           break;
       }
 
       switch (this->action_.ai_launcher) {
         case FIRE:
-          this->event_.Active(AI_FIRE_COMMAND);  //连续发弹
+          this->event_.Active(AI_FIRE_COMMAND); /* 发弹指令，采用连发模式 */
           break;
         case CEASEFIRE:
           this->event_.Active(AI_STOP_FIRE);
@@ -322,7 +312,7 @@ bool AI::PackCMD() {
           cmd_for_ref_.confirm_resurrection = 0;
           cmd_for_ref_.buy_resurrection = 0;
           cmd_for_ref_.buy_bullet_num = this->last_buy_bullet_num_;
-          cmd_for_ref_.remote_buy_bullet_times = 2;
+          cmd_for_ref_.remote_buy_bullet_times = 0;
           cmd_for_ref_.romote_buy_hp_times = 0;
           break;
       }
diff --git a/src/device/ai/dev_ai.hpp b/src/device/ai/dev_ai.hpp
index 93de344b..6a94937d 100644
--- a/src/device/ai/dev_ai.hpp
+++ b/src/device/ai/dev_ai.hpp
@@ -50,10 +50,10 @@ class AI {
     float scanf_pit_omega;
   } ScanfMode;
   typedef struct {
-    float yaw_for_chassis;
+    float yaw_; /* (-PI,+PI) */
     ScanfMode scanf_mode;
     float get_yaw_angle;
-    Component::Type::CycleValue start_imu_yaw;
+    Component::Type::Eulr mech_zero;
   } GimbalData;
 
   typedef enum {
@@ -74,23 +74,22 @@ class AI {
     GAME_END = 5,
   } GameProgress;
   typedef enum {
-    TO_BASE,        // 0
-    TO_OUTPOST,     // 1
-    TO_SUPPLY,      // 2
-    TO_HIGHLAND,    // 3
-    TO_PATROL_AREA, /* 基地前巡逻区 */
-    ROTOR,          // 5
-
-    SCANF,     // 6
-    AUTO_AIM,  // 7
-    AFFECTED,  /* 反击*/
-
-    CEASEFIRE, /* 停火 */
-    FIRE,      // 10
-
-    NOTHING,              // 11
-    CONFIRM_RESURRECTION, /* 确认复活 */
-    EXCHANGE_BULLETS,     /* 兑换弹丸 */
+    TO_PATROL_AREA = 0, /* 哨兵巡逻区 */
+    TO_SUPPLY = 1,      /* 补给区 */
+    TO_HIGHWAY = 2,     /* 公路区 */
+    TO_OUTPOST = 3,     /* 前哨站 */
+    ROTOR = 4,
+
+    SCANF = 5,
+    AUTO_AIM = 6,
+    AFFECTED = 7, /* 反击*/
+
+    CEASEFIRE = 8, /* 停火 */
+    FIRE = 9,
+
+    NOTHING = 10,
+    CONFIRM_RESURRECTION = 11, /* 确认复活 */
+    EXCHANGE_BULLETS = 12,     /* 兑换弹丸 */
     /* 其他行为暂不考虑 */
   } Action;
 
@@ -189,6 +188,8 @@ class AI {
 
   Component::Type::Eulr eulr_;
 
+  Component::Type::CycleValue imu_yaw_now_;
+
   struct {
     float yaw; /* 偏航角（Yaw angle） */
     float pit; /* 俯仰角（Pitch angle） */
-- 
Gitee


From 95bb2dacd5d143dc79ada0bfa3c0388ac3cfa448 Mon Sep 17 00:00:00 2001
From: Marine_zxy <1423266892@qq.com>
Date: Wed, 15 May 2024 22:49:48 +0800
Subject: [PATCH 17/19] =?UTF-8?q?=E6=96=B0=E5=A2=9E=E5=8A=9F=E7=8E=87?=
 =?UTF-8?q?=E6=8E=A7=E5=88=B6=E7=9B=B8=E5=85=B3=E5=8F=98=E9=87=8F=E3=80=81?=
 =?UTF-8?q?=E5=91=BD=E5=90=8D=E4=BF=AE=E6=AD=A3?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/device/motor/dev_motor.hpp     |  2 ++
 src/module/chassis/mod_chassis.cpp | 11 ++++++-----
 src/module/chassis/mod_chassis.hpp |  6 +++++-
 src/module/gimbal/mod_gimbal.cpp   | 11 ++++++-----
 src/module/gimbal/mod_gimbal.hpp   |  3 ++-
 5 files changed, 21 insertions(+), 12 deletions(-)

diff --git a/src/device/motor/dev_motor.hpp b/src/device/motor/dev_motor.hpp
index 723ace9b..716c9ccc 100644
--- a/src/device/motor/dev_motor.hpp
+++ b/src/device/motor/dev_motor.hpp
@@ -11,6 +11,8 @@ class BaseMotor {
     float rotational_speed = 0.0f; /* 转速 单位：rpm */
     float torque_current = 0.0f;   /* 转矩电流 单位：A*/
     float temp = 0.0f;             /* 电机温度 单位：℃*/
+    float motor_speed[4];
+    float pos_speed[4];
   } Feedback;
 
   BaseMotor(const char *name, bool reverse)
diff --git a/src/module/chassis/mod_chassis.cpp b/src/module/chassis/mod_chassis.cpp
index d3c3f14d..2f10204a 100644
--- a/src/module/chassis/mod_chassis.cpp
+++ b/src/module/chassis/mod_chassis.cpp
@@ -134,6 +134,7 @@ void Chassis<Motor, MotorParam>::UpdateFeedback() {
   /* 将CAN中的反馈数据写入到feedback中 */
   for (size_t i = 0; i < this->mixer_.len_; i++) {
     this->motor_[i]->Update();
+    this->motor_feedback_.motor_speed[i] = this->motor_[i]->GetSpeed();
   }
 }
 
@@ -163,7 +164,7 @@ void Chassis<Motor, MotorParam>::Control() {
     case Chassis::FOLLOW_GIMBAL: /* 按照云台方向换算运动向量
                                   */
     case Chassis::ROTOR: {
-      float beta = this->gimbal_data_.yaw_for_chassis;
+      float beta = this->gimbal_data_.yaw_;
       float cos_beta = cosf(beta);
       float sin_beta = sinf(beta);
       this->move_vec_.vx = cos_beta * this->cmd_.x - sin_beta * this->cmd_.y;
@@ -184,8 +185,8 @@ void Chassis<Motor, MotorParam>::Control() {
 
     case Chassis::FOLLOW_GIMBAL: /* 跟随模式通过PID控制使车头跟随云台
                                   */
-      this->move_vec_.wz = this->follow_pid_.Calculate(
-          0.0f, this->gimbal_data_.yaw_for_chassis, this->dt_);
+      this->move_vec_.wz =
+          this->follow_pid_.Calculate(0.0f, this->gimbal_data_.yaw_, this->dt_);
       break;
 
     case Chassis::ROTOR: { /* 小陀螺模式使底盘以一定速度旋转
@@ -222,11 +223,11 @@ void Chassis<Motor, MotorParam>::Control() {
       clampf(&percentage, 0.0f, 1.0f);
 
       for (unsigned i = 0; i < this->mixer_.len_; i++) {
-        float out = this->actuator_[i]->Calculate(
+        out_.motor3508_out[i] = this->actuator_[i]->Calculate(
             this->setpoint_.motor_rotational_speed[i] *
                 MOTOR_MAX_ROTATIONAL_SPEED,
             this->motor_[i]->GetSpeed(), this->dt_);
-        this->motor_[i]->Control(out * percentage);
+        this->motor_[i]->Control(out_.motor3508_out[i]);
       }
 
       break;
diff --git a/src/module/chassis/mod_chassis.hpp b/src/module/chassis/mod_chassis.hpp
index 197759dd..1e749b14 100644
--- a/src/module/chassis/mod_chassis.hpp
+++ b/src/module/chassis/mod_chassis.hpp
@@ -44,7 +44,7 @@ class Chassis {
   } ChassisEvent;
 
   typedef struct {
-    float yaw_for_chassis;
+    float yaw_;
     Component::Type::Eulr mech_zero;
   } GimbalData;
 
@@ -120,6 +120,9 @@ class Chassis {
 
   /* 反馈控制用的PID */
 
+  struct {
+    float motor3508_out[4];
+  } out_;
   Component::PID follow_pid_; /* 跟随云台用的PID */
 
   System::Thread thread_;
@@ -128,6 +131,7 @@ class Chassis {
 
   GimbalData gimbal_data_;
   Device::Referee::Data raw_ref_;
+  Device::BaseMotor::Feedback motor_feedback_;
   Component::CMD::ChassisCMD cmd_;
 
   Component::UI::String string_;
diff --git a/src/module/gimbal/mod_gimbal.cpp b/src/module/gimbal/mod_gimbal.cpp
index 5ea2f8b3..23c36238 100644
--- a/src/module/gimbal/mod_gimbal.cpp
+++ b/src/module/gimbal/mod_gimbal.cpp
@@ -81,12 +81,13 @@ void Gimbal::UpdateFeedback() {
   switch (this->mode_) {
     case RELAX:
     case ABSOLUTE:
-      this->gimbal_data_.yaw_for_chassis =
+      this->gimbal_data_.yaw_ =
           this->yaw_motor_.GetAngle() - this->param_.mech_zero.yaw;
       break;
   }
   this->gimbal_data_.scanf_mode = this->param_.scanf_mode;
   this->gimbal_data_.get_yaw_angle = this->yaw_motor_.GetAngle();
+  this->gimbal_data_.mech_zero = this->param_.mech_zero;
 }
 
 void Gimbal::Control() {
@@ -251,9 +252,9 @@ void Gimbal::DrawUIStatic(Gimbal* gimbal) {
       static_cast<uint16_t>(Device::Referee::UIGetWidth() * 0.4f),
       static_cast<uint16_t>(Device::Referee::UIGetHeight() * 0.2f),
       static_cast<uint16_t>(Device::Referee::UIGetWidth() * 0.4f +
-                            -sinf(gimbal->gimbal_data_.yaw_for_chassis) * 44),
+                            -sinf(gimbal->gimbal_data_.yaw_) * 44),
       static_cast<uint16_t>(Device::Referee::UIGetHeight() * 0.2f +
-                            cosf(gimbal->gimbal_data_.yaw_for_chassis) * 44));
+                            cosf(gimbal->gimbal_data_.yaw_) * 44));
   Device::Referee::AddUI(gimbal->line_);
 }
 
@@ -306,8 +307,8 @@ void Gimbal::DrawUIDynamic(Gimbal* gimbal) {
       static_cast<uint16_t>(Device::Referee::UIGetWidth() * 0.4f),
       static_cast<uint16_t>(Device::Referee::UIGetHeight() * 0.2f),
       static_cast<uint16_t>(Device::Referee::UIGetWidth() * 0.4f +
-                            -sinf(gimbal->gimbal_data_.yaw_for_chassis) * 44),
+                            -sinf(gimbal->gimbal_data_.yaw_) * 44),
       static_cast<uint16_t>(Device::Referee::UIGetHeight() * 0.2f +
-                            cosf(gimbal->gimbal_data_.yaw_for_chassis) * 44));
+                            cosf(gimbal->gimbal_data_.yaw_) * 44));
   Device::Referee::AddUI(gimbal->line_);
 }
diff --git a/src/module/gimbal/mod_gimbal.hpp b/src/module/gimbal/mod_gimbal.hpp
index ae7eadda..e7160633 100644
--- a/src/module/gimbal/mod_gimbal.hpp
+++ b/src/module/gimbal/mod_gimbal.hpp
@@ -45,9 +45,10 @@ class Gimbal {
     float scanf_pit_omega;
   } ScanfMode;
   typedef struct {
-    float yaw_for_chassis;
+    float yaw_; /* (-PI,+PI) */
     ScanfMode scanf_mode;
     float get_yaw_angle;
+    Component::Type::Eulr mech_zero;
   } GimbalData;
 
   typedef struct {
-- 
Gitee


From 9ad4c2bf3dc6b5cab4ebd00b8f685613d98b96e7 Mon Sep 17 00:00:00 2001
From: Marine_zxy <1423266892@qq.com>
Date: Wed, 15 May 2024 22:51:44 +0800
Subject: [PATCH 18/19] =?UTF-8?q?=E5=93=A8=E5=85=B5=E5=8F=82=E6=95=B0?=
 =?UTF-8?q?=E4=BF=AE=E6=94=B9?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/robot/sentry/robot.cpp | 71 ++++++++++++--------------------------
 1 file changed, 23 insertions(+), 48 deletions(-)

diff --git a/src/robot/sentry/robot.cpp b/src/robot/sentry/robot.cpp
index 1589928e..dab1cd9f 100644
--- a/src/robot/sentry/robot.cpp
+++ b/src/robot/sentry/robot.cpp
@@ -9,10 +9,10 @@
 /* clang-format off */
 Robot::Sentry::Param param = {
     .chassis={
-      .type = Component::Mixer::OMNICROSS,
+      .type = Component::Mixer::MECANUM,
 
       .follow_pid_param = {
-      .k = 0.8f,
+      .k = 0.5f,
       .p = 1.0f,
       .i = 0.0f,
       .d = 0.0f,
@@ -29,7 +29,7 @@ Robot::Sentry::Param param = {
       },
       Component::CMD::EventMapItem{
         Device::DR16::DR16_SW_L_POS_TOP,
-        Module::RMChassis::SET_MODE_RELAX
+        Module::RMChassis::SET_MODE_INDENPENDENT
       },
       Component::CMD::EventMapItem{
         Device::DR16::DR16_SW_L_POS_MID,
@@ -40,15 +40,7 @@ Robot::Sentry::Param param = {
         Module::RMChassis::SET_MODE_ROTOR
       },
        Component::CMD::EventMapItem{
-        Device::AI::AI_INDENPENDENT,
-        Module::RMChassis::SET_MODE_INDENPENDENT
-      },
-       Component::CMD::EventMapItem{
-        Device::AI::AI_FOLLOW,
-        Module::RMChassis::SET_MODE_FOLLOW
-      },
-      Component::CMD::EventMapItem{
-        Device::AI::AI_ROTOR,
+        Device::AI::AIControlData::AI_ROTOR,
         Module::RMChassis::SET_MODE_ROTOR
       }
     },
@@ -56,7 +48,7 @@ Robot::Sentry::Param param = {
     .actuator_param = {
       Component::SpeedActuator::Param{
         .speed = {
-          .k = 0.00020f,
+          .k = 0.00015f,
           .p = 1.0f,
           .i = 0.0f,
           .d = 0.0f,
@@ -89,7 +81,7 @@ Robot::Sentry::Param param = {
       },
       Component::SpeedActuator::Param{
         .speed = {
-          .k = 0.00020f,
+          .k = 0.00015f,
           .p = 1.0f,
           .i = 0.0f,
           .d = 0.0f,
@@ -105,7 +97,7 @@ Robot::Sentry::Param param = {
       },
       Component::SpeedActuator::Param{
         .speed = {
-          .k = 0.00022f,
+          .k = 0.00015f,
           .p = 1.0f,
           .i = 0.0f,
           .d = 0.0f,
@@ -171,7 +163,7 @@ Robot::Sentry::Param param = {
     .yaw_actr = {
       .speed = {
           /* GIMBAL_CTRL_YAW_OMEGA_IDX */
-          .k = 0.20f,
+          .k = 0.28f,
           .p = 1.f,
           .i = 1.f,
           .d = 0.f,
@@ -183,7 +175,7 @@ Robot::Sentry::Param param = {
 
         .position = {
           /* GIMBAL_CTRL_YAW_ANGLE_IDX */
-          .k = 20.0f,
+          .k = 25.0f,
           .p = 1.0f,
           .i = 0.0f,
           .d = 0.0f,
@@ -246,7 +238,7 @@ Robot::Sentry::Param param = {
       .rol = 0.0f,
     },
     .scanf_mode{
-      .scanf_yaw_rate = 0.005f,
+      .scanf_yaw_rate = 0.0025f,
       .scanf_pit_center = 0.04f,
       .scanf_pit_range = 0.22f,
       .scanf_pit_omega = 5.0f,
@@ -293,7 +285,7 @@ Robot::Sentry::Param param = {
         .speed = {
           .k = 3.0f,
           .p = 1.0f,
-          .i = 0.5f,
+          .i = 0.0f,
           .d = 0.0f,
           .i_limit = 0.5f,
           .out_limit = 1.0f,
@@ -355,7 +347,7 @@ Robot::Sentry::Param param = {
 
     .trig_motor = {
       Device::RMMotor::Param{
-        .id_feedback = 0x206,
+        .id_feedback = 0x207,
         .id_control = M3508_M2006_CTRL_ID_EXTAND,
         .model = Device::RMMotor::MOTOR_M2006,
         .can = BSP_CAN_2,
@@ -379,7 +371,7 @@ Robot::Sentry::Param param = {
     },
 
     .EVENT_MAP = {
-      Component::CMD::EventMapItem{
+       Component::CMD::EventMapItem{
         Component::CMD::CMD_EVENT_LOST_CTRL,
         Module::Launcher::CHANGE_FIRE_MODE_RELAX
       },
@@ -388,25 +380,17 @@ Robot::Sentry::Param param = {
         Module::Launcher::CHANGE_FIRE_MODE_SAFE
       },
       Component::CMD::EventMapItem{
-        Device::DR16::DR16_SW_R_POS_MID,/* 模拟找到目标模式，云台绝对 */
+        Device::DR16::DR16_SW_R_POS_MID,
         Module::Launcher::CHANGE_FIRE_MODE_LOADED
       },
       Component::CMD::EventMapItem{
-        Device::DR16::DR16_SW_R_POS_BOT,/* 模拟未找到目标，巡逻模式 */
+        Device::DR16::DR16_SW_R_POS_BOT,
         Module::Launcher::CHANGE_FIRE_MODE_LOADED
       },
       Component::CMD::EventMapItem{
-        Device::DR16::DR16_SW_R_POS_BOT,/* 模拟未找到目标，巡逻模式 */
+        Device::DR16::DR16_SW_R_POS_BOT,
         Module::Launcher::LAUNCHER_START_FIRE
       },
-       Component::CMD::EventMapItem{
-        Device::DR16::DR16_SW_R_POS_BOT,/* 模拟未找到目标，巡逻模式 */
-        Module::Launcher::CHANGE_TRIG_MODE_BURST
-      },
-      //   Component::CMD::EventMapItem{
-      //   Device::AI::AIControlData::AI_FIRE_COMMAND,
-      //   Module::Launcher::LAUNCHER_START_FIRE
-      // },
       Component::CMD::EventMapItem{
         Device::AI::AIControlData::AI_FIRE_COMMAND,
         Module::Launcher::CHANGE_TRIG_MODE_BURST
@@ -433,7 +417,7 @@ Robot::Sentry::Param param = {
         .speed = {
           .k = 3.0f,
           .p = 1.0f,
-          .i = 0.5f,
+          .i = 0.0f,
           .d = 0.0f,
           .i_limit = 0.5f,
           .out_limit = 1.0f,
@@ -495,7 +479,7 @@ Robot::Sentry::Param param = {
 
     .trig_motor = {
       Device::RMMotor::Param{
-        .id_feedback =0x207,
+        .id_feedback =0x206,
         .id_control = M3508_M2006_CTRL_ID_EXTAND,
         .model = Device::RMMotor::MOTOR_M2006,
         .can = BSP_CAN_2,
@@ -520,8 +504,8 @@ Robot::Sentry::Param param = {
       },
     },
 
-    .EVENT_MAP = {
-      Component::CMD::EventMapItem{
+     .EVENT_MAP = {
+       Component::CMD::EventMapItem{
         Component::CMD::CMD_EVENT_LOST_CTRL,
         Module::Launcher::CHANGE_FIRE_MODE_RELAX
       },
@@ -530,26 +514,17 @@ Robot::Sentry::Param param = {
         Module::Launcher::CHANGE_FIRE_MODE_SAFE
       },
       Component::CMD::EventMapItem{
-        Device::DR16::DR16_SW_R_POS_MID,/* 模拟找到目标模式，云台绝对 */
+        Device::DR16::DR16_SW_R_POS_MID,
         Module::Launcher::CHANGE_FIRE_MODE_LOADED
       },
       Component::CMD::EventMapItem{
-        Device::DR16::DR16_SW_R_POS_BOT,/* 模拟未找到目标，巡逻模式 */
+        Device::DR16::DR16_SW_R_POS_BOT,
         Module::Launcher::CHANGE_FIRE_MODE_LOADED
       },
       Component::CMD::EventMapItem{
-        Device::DR16::DR16_SW_R_POS_BOT,/* 模拟未找到目标，巡逻模式 */
+        Device::DR16::DR16_SW_R_POS_BOT,
         Module::Launcher::LAUNCHER_START_FIRE
       },
-       Component::CMD::EventMapItem{
-        Device::DR16::DR16_SW_R_POS_BOT,/* 模拟未找到目标，巡逻模式 */
-        Module::Launcher::CHANGE_TRIG_MODE_BURST
-      },
-
-      //   Component::CMD::EventMapItem{
-      //   Device::AI::AIControlData::AI_FIRE_COMMAND,
-      //   Module::Launcher::LAUNCHER_START_FIRE
-      // },
       Component::CMD::EventMapItem{
         Device::AI::AIControlData::AI_FIRE_COMMAND,
         Module::Launcher::CHANGE_TRIG_MODE_BURST
-- 
Gitee


From cea46bcfcb54c4b4f599b2d536efe47c28103170 Mon Sep 17 00:00:00 2001
From: Marine_zxy <1423266892@qq.com>
Date: Wed, 15 May 2024 22:53:58 +0800
Subject: [PATCH 19/19]

---
 src/device/ai/dev_ai.hpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/device/ai/dev_ai.hpp b/src/device/ai/dev_ai.hpp
index 6a94937d..c5a03ba7 100644
--- a/src/device/ai/dev_ai.hpp
+++ b/src/device/ai/dev_ai.hpp
@@ -129,7 +129,7 @@ class AI {
   void DecideAction();
 
  private:
-  bool autoscan_enable_ = true; /* ai自动扫描，不启用则默认ai全程在线 */
+  bool autoscan_enable_ = true; /* AI自动扫描，不启用则默认AI全程在线 */
 
   bool ref_updated_ = false;
 
-- 
Gitee