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 0000000000000000000000000000000000000000..fba87b3cfff5f4c01297367185ab6598c6a22395
--- /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/component/comp_cmd.hpp b/src/component/comp_cmd.hpp
index b96c9c6d7dba6fd68a48ec689d583e36b41a9544..be3b97e010c944f550fe62f3e646280703657cb4 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,
@@ -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/component/comp_mixer.cpp b/src/component/comp_mixer.cpp
index 301ed737d206188ef78a0fe8cbd5e6b13e1f5fa8..ead64bacd4af3a190b6f207a1ec338d2ff0d961d 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;
   }
diff --git a/src/device/ai/dev_ai.cpp b/src/device/ai/dev_ai.cpp
index 4e2fb2a8d67dccb938f51f6b1d844a74aa219b55..c52328169479c491fd0f6ff9f41c683609792a74 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) {
@@ -34,29 +35,37 @@ 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");
+
+    auto eulr_sub = Message::Subscriber<Component::Type::Eulr>("imu_eulr");
 
     while (1) {
-      /* 接收指令 */
-      ai->StartRecv();
+      /* 接收云台数据 */
+      gimbal_sub.DumpData(ai->gimbal_data_);
+      eulr_sub.DumpData(ai->eulr_); /* imu */
 
+      /* 接收裁判系统数据 */
+      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);
@@ -101,7 +110,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_);
   }
@@ -112,6 +125,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);
@@ -135,50 +149,207 @@ bool AI::PackRef() {
   return true;
 }
 
-bool AI::PackCMD() {
+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;
+  } else {
+    this->aim_time_ = bsp_time_get_ms(); /* 自瞄锁定时刻 */
+  }
+  memcpy(&(this->last_eulr_), &(this->cmd_.gimbal.eulr),
+         sizeof(this->cmd_.gimbal.eulr));
 
-  memcpy(&(this->cmd_.ext.extern_channel),
-         &(this->from_host_.data.extern_channel),
-         sizeof(this->cmd_.ext.extern_channel));
+  /* 判定是否受击打，一定时间内第一次受击打方向优先级最高 */
+  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();
+  }
+  if (bsp_time_get_ms() - this->damaged_time_ > 1500) {
+    is_damaged_ = false;
+  }
+  this->last_damage_type_ = this->raw_ref_.robot_damage.damage_type;
 
-  memcpy(&(this->cmd_.chassis), &(this->from_host_.data.chassis_move_vec),
-         sizeof(this->from_host_.data.chassis_move_vec));
+  /* 判定是否可以开始导航 */
+  if (this->from_host_.data.chassis_move_vec.vx == 0 && is_damaged_) {
+    can_start_navigation_ = false;
+  } else {
+    can_start_navigation_ = true;
+  }
 
-  this->notice_ = this->from_host_.data.notice;
+  /* 裁判系统行为：确认复活、购买发弹量 */
+  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;
+  }
 
-  memcpy(&(this->last_eulr_), &(this->cmd_.gimbal.eulr),
-         sizeof(this->cmd_.gimbal.eulr));
+  /* 底盘行为*/
+  if (can_start_navigation_) {
+    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 < 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;
+      }
+    }
+  } else {
+    this->action_.ai_chassis = AI::Action::ROTOR;
+  }
 
-  this->cmd_.ctrl_source = Component::CMD::CTRL_SOURCE_AI;
+  /* 云台行为 */
+  if (this->notice_ == AI_NOTICE_AUTO_AIM || this->notice_ == AI_NOTICE_FIRE ||
+      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_) {
+    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;
+  }
 
-  this->cmd_tp_.Publish(this->cmd_);
+  /* 发射机构行为 */
+  if (this->notice_ == AI_NOTICE_FIRE) { /* 开火 */
+    this->action_.ai_launcher = AI::Action::FIRE;
+  } else {
+    this->action_.ai_launcher = AI::Action::CEASEFIRE; /* 不发弹 */
+  }
+}
 
+bool AI::PackCMD() {
+  /* 确保遥控器开关最高控制权，关遥控器即断控 */
   if (!Component::CMD::Online()) {
     return false;
   }
 
-  /* 控制权在AI */
+  /* 控制源：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);
+    /* AUTO控制模式，用于全自动机器人 */
+    if (Component::CMD::GetCtrlMode() == Component::CMD::CMD_AUTO_CTRL) {
+      switch (this->action_.ai_chassis) {
+        case TO_PATROL_AREA:
+        case TO_HIGHWAY:
+        case TO_OUTPOST:
+        case TO_SUPPLY:
+          memcpy(&(this->cmd_.chassis),
+                 &(this->from_host_.data.chassis_move_vec),
+                 sizeof(this->from_host_.data.chassis_move_vec));
+          break;
+        case ROTOR:
+          this->event_.Active(AI_ROTOR);
+          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->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->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);
+          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 = 0;
+          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_);
       }
-    } else {
-      this->event_.Active(AI_OFFLINE);
     }
-  }
+    /* 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));
+
+      memcpy(&(this->cmd_.ext.extern_channel),
+             &(this->from_host_.data.extern_channel),
+             sizeof(this->cmd_.ext.extern_channel));
 
-  this->notice_ = 0;
+      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;
+
+      this->cmd_tp_.Publish(this->cmd_);
+      this->cmd_tp_.Publish(this->cmd_);
+      this->cmd_tp_.Publish(this->cmd_);
+    }
+  }
   return true;
 }
 
@@ -245,4 +416,29 @@ void AI::PraseRef() {
     default:
       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;
+    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 bdbc56a69537efb1d55634d62e27f8317b101c40..c5a03ba7c691e68e5bc9cbec65c064b8a1abdbdf 100644
--- a/src/device/ai/dev_ai.hpp
+++ b/src/device/ai/dev_ai.hpp
@@ -29,17 +29,86 @@ class AI {
     uint32_t ball_speed;
     uint32_t max_hp;
     uint32_t hp;
+
+    uint8_t game_progress;
+    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 struct {
+    float scanf_yaw_rate;
+    float scanf_pit_center;
+    float scanf_pit_range;
+    float scanf_pit_omega;
+  } ScanfMode;
+  typedef struct {
+    float yaw_; /* (-PI,+PI) */
+    ScanfMode scanf_mode;
+    float get_yaw_angle;
+    Component::Type::Eulr mech_zero;
+  } 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;
 
-  AI();
+  typedef enum {
+    WAITTING = 0,
+    PREPARATION = 1,
+    REF_INSPECTION = 2,
+    COUNTDOWN_5S = 3,
+    GAMING = 4,
+    GAME_END = 5,
+  } GameProgress;
+  typedef enum {
+    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;
+
+  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;     /* 扫描、自瞄、反击 */
+    uint8_t ai_launcher;   /* 不发弹丸、开火 */
+    uint8_t ai_to_referee; /* 确认复活、购买发弹量、购买血量 */
+  } AICtrlAction;
+
+  AI(bool autoscan_enable = false);
 
   bool StartRecv();
 
@@ -57,7 +126,11 @@ class AI {
 
   bool PackCMD();
 
+  void DecideAction();
+
  private:
+  bool autoscan_enable_ = true; /* AI自动扫描，不启用则默认AI全程在线 */
+
   bool ref_updated_ = false;
 
   uint32_t last_online_time_ = 0;
@@ -66,6 +139,8 @@ class AI {
 
   uint8_t notice_;
 
+  bool can_start_navigation_ = 0;
+
   struct {
     RefereePckage ref{};
     MCUPckage mcu{};
@@ -73,6 +148,13 @@ class AI {
 
   RefForAI ref_{};
 
+  uint8_t last_damage_type_;
+  bool is_damaged_;
+  uint8_t damaged_id_;
+  uint32_t damaged_time_;
+  Component::Type::CycleValue damaged_yaw_;
+
+  uint32_t aim_time_;
   System::Thread thread_;
 
   System::Semaphore data_ready_;
@@ -83,13 +165,35 @@ class AI {
 
   Component::CMD::Data cmd_{};
 
+  AICtrlAction action_;
+
+  Component::Type::Eulr mech_zero_;
+
+  GimbalData gimbal_data_;
+
+  float last_yaw_angle_;
+
+  SentryDecisionData cmd_for_ref_;
+
+  uint8_t last_buy_bullet_num_ = 0;
+  Component::Type::CycleValue target_angle_ = 0.0;
+
+  Message::Topic<SentryDecisionData> ai_tp_ =
+      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_;
+
+  Component::Type::CycleValue imu_yaw_now_;
+
   struct {
     float yaw; /* 偏航角（Yaw angle） */
     float pit; /* 俯仰角（Pitch angle） */
     float rol; /* 翻滚角（Roll angle） */
   } last_eulr_;
-
-  Component::Type::Quaternion quat_{};
-  Device::Referee::Data raw_ref_{};
 };
 }  // namespace Device
diff --git a/src/device/motor/dev_motor.hpp b/src/device/motor/dev_motor.hpp
index 723ace9b24aadb489bd007e2a15b35dd960a5108..716c9cccec74dff002d57dc3cc72085ba72abfb1 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/device/referee/dev_referee.cpp b/src/device/referee/dev_referee.cpp
index 5f2009101b3f4d182549a8c06490493333c8d30a..e0fef17debb8f23c81aae5b77931d309fbfdafc7 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,11 +99,19 @@ 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) {
-      ref->UpdateUI();
+      ai_sub.DumpData(ref->data_from_sentry_);
+      ref->UpdateUI(); /* 更新UI */
+                       // 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);
     }
   };
@@ -318,6 +328,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,14 +388,12 @@ 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);
     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);
@@ -436,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));
@@ -479,14 +488,88 @@ 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();
 
   return true;
 }
 
+bool Referee::SentryDecision() {
+  uint32_t pack_size = 0;
+  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
+
+  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_.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;
+}
+
 bool Referee::AddUI(Component::UI::Ele ui_data) {
   self_->ui_lock_.Wait(UINT32_MAX);
   if (ui_data.op == Component::UI::UI_GRAPHIC_OP_ADD) {
@@ -530,6 +613,17 @@ 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;
+  if (robot_id > 100) {
+    header.id_sender = 0x0107;
+  } else {
+    header.id_sender = 0x0007;
+  }
+  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 a7a76e1227b1ea0881dd7e209d479db92f80c55e..2120f4344c9de99cb13d24235a1a99c6b9a80360 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
@@ -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,11 +288,11 @@ 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;
-    /*最大值113*/
+    std::array<int8_t, 112> user_data;
+    /*最大值112*/
   } RobotInteractionData; /* 0x0301 */
 
   typedef struct __attribute__((packed)) {
@@ -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)) {
@@ -487,9 +494,33 @@ 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)) {
+    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;  //含0x0120
+    // SentryDecisionData sentry_cmd;
+    uint32_t data_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();
 
@@ -503,6 +534,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);
@@ -516,6 +549,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);
 
@@ -546,8 +581,13 @@ 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);
+
   Data ref_data_;
 
   Data last_data_;
@@ -556,6 +596,9 @@ class Referee {
 
   static UIPack ui_pack_;
 
+  static SentryPack sentry_pack_;
+
   static Referee *self_;
+  SentryDecisionData data_from_sentry_;
 };
 }  // namespace Device
diff --git a/src/module/chassis/mod_chassis.cpp b/src/module/chassis/mod_chassis.cpp
index f4c8c190b3b4b5ccd621dd63de5905e459488f3b..2f10204a1ab56e9bf4ad85daa2dcbe5b28958e8f 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_);
 
       /* 更新反馈值 */
@@ -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->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 +186,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: { /* 小陀螺模式使底盘以一定速度旋转
@@ -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 f5f435c4bdaba8be43865d0a8640caa929ac4da6..1e749b14b51ed86916508bfc06cc02b267ffbe0d 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 =
@@ -115,14 +120,18 @@ class Chassis {
 
   /* 反馈控制用的PID */
 
+  struct {
+    float motor3508_out[4];
+  } out_;
   Component::PID follow_pid_; /* 跟随云台用的PID */
 
   System::Thread thread_;
 
   System::Semaphore ctrl_lock_;
 
-  float yaw_;
+  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 f7e76372dc5dd78cf65fcd23b302a2d630282588..23c3623811f46a2757c83970d09c32089fd0e620 100644
--- a/src/module/gimbal/mod_gimbal.cpp
+++ b/src/module/gimbal/mod_gimbal.cpp
@@ -33,9 +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;
     }
     gimbal->ctrl_lock_.Post();
   };
@@ -63,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);
@@ -84,18 +81,13 @@ void Gimbal::UpdateFeedback() {
   switch (this->mode_) {
     case RELAX:
     case ABSOLUTE:
-      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_.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() {
@@ -118,7 +110,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) {
@@ -149,7 +140,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();
@@ -167,24 +157,6 @@ void Gimbal::Control() {
       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;
   }
 }
 
@@ -204,14 +176,13 @@ void Gimbal::SetMode(Mode mode) {
       this->setpoint_.eulr_.yaw = this->eulr_.yaw;
     }
   }
-
-  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,
@@ -281,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_);
 }
 
@@ -336,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 953677f2fe5560633a3799fd12c250c4beadd103..e71606331f1aa4a2b6e84a9283089c7120081b45 100644
--- a/src/module/gimbal/mod_gimbal.hpp
+++ b/src/module/gimbal/mod_gimbal.hpp
@@ -20,8 +20,7 @@ class Gimbal {
   /* 云台运行模式 */
   typedef enum {
     RELAX, /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */
-    ABSOLUTE,   /* 绝对坐标系控制，控制在空间内的绝对姿态 */
-    AUTOPATROL, /* 自动巡逻模式，云台yaw轴按sin曲线进行扫描 */
+    ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */
   } Mode;
 
   enum {
@@ -37,9 +36,21 @@ class Gimbal {
     SET_MODE_ABSOLUTE,
     START_AUTO_AIM,
     STOP_AUTO_AIM,
-    SET_AUTOPATROL,
   } GimbalEvent;
 
+  typedef struct {
+    float scanf_yaw_rate;
+    float scanf_pit_center;
+    float scanf_pit_range;
+    float scanf_pit_omega;
+  } ScanfMode;
+  typedef struct {
+    float yaw_; /* (-PI,+PI) */
+    ScanfMode scanf_mode;
+    float get_yaw_angle;
+    Component::Type::Eulr mech_zero;
+  } GimbalData;
+
   typedef struct {
     Component::SecOrderFunction::Param ff; /* PITCH前馈 */
     Component::SecOrderFunction::Param st; /* YAW自整定参数 */
@@ -52,9 +63,7 @@ class Gimbal {
 
     Component::Type::Eulr mech_zero;
 
-    float patrol_range;
-    float patrol_omega;
-    float patrol_hight;
+    ScanfMode scanf_mode;
 
     struct {
       Component::Type::CycleValue pitch_max;
@@ -66,6 +75,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 +88,8 @@ class Gimbal {
 
   void SetMode(Mode mode);
 
+  void PraseRef();
+
   static void DrawUIStatic(Gimbal *gimbal);
 
   static void DrawUIDynamic(Gimbal *gimbal);
@@ -86,8 +101,6 @@ class Gimbal {
 
   float dt_ = 0.0f;
 
-  uint32_t autopatrol_start_time_ = 0;
-
   Param param_;
 
   Gimbal::Mode mode_ = RELAX; /* 云台模式 */
@@ -104,13 +117,17 @@ class Gimbal {
   Device::RMMotor yaw_motor_;
   Device::RMMotor pit_motor_;
 
+  Device::Referee::Data raw_ref_;
+  RefForGImbal ref_;
+
   System::Thread thread_;
 
   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/module/launcher/mod_launcher.cpp b/src/module/launcher/mod_launcher.cpp
index 961485cea2c746e3a91a334daa9da6e42aeb2559..838cb9ab6261fd5ef5fe60864911a463dcfa7af5 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 b7e0c773f1ddfef0d451dd526d142b413b11fb76..05808e9a1ac9deec988b8f1bbbcd6b7cf56e4138 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/module/mit_gimbal/Kconfig b/src/module/mit_gimbal/Kconfig
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/module/mit_gimbal/info.cmake b/src/module/mit_gimbal/info.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..35463fe0f50e9bf2811f6765ca1cf37ab252bcea
--- /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 0000000000000000000000000000000000000000..119bf20358f20733dba42006bc827b234c42fe27
--- /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 0000000000000000000000000000000000000000..f1a0e37539be872ccd19a34d5734112ce1059ad2
--- /dev/null
+++ b/src/module/mit_gimbal/mod_mit_gimbal.hpp
@@ -0,0 +1,137 @@
+#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_mit_motor.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::MitMotor::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::MitMotor 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 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/robot/omni_infantry/robot.cpp b/src/robot/omni_infantry/robot.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b3246de70e75189310064c8adab470dad41a9757
--- /dev/null
+++ b/src/robot/omni_infantry/robot.cpp
@@ -0,0 +1,440 @@
+#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 = {
+      .kp = 0.0f,
+      .kd = 0.0f,
+      .def_speed = 0.0,
+      .id = 0x0,
+      .can = BSP_CAN_2,
+      .max_error = 0.0,
+      .reverse = false,
+    },
+
+    .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 0000000000000000000000000000000000000000..2a0f8a40dc21439e15ceabc70b02d6a56a910c00
--- /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
diff --git a/src/robot/sentry/robot.cpp b/src/robot/sentry/robot.cpp
index fa5183f56238ced11af291a2bcc2c22334bd4cee..dab1cd9f5e66b67283b1f848e4f922979dd0c26c 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,30 +29,18 @@ 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,/* 模拟找到目标模式，云台绝对 */
-        Module::RMChassis::SET_MODE_FOLLOW,
+        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
-      },
-      Component::CMD::EventMapItem{
-        Device::AI::AI_FIND_TARGET,
+        Device::DR16::DR16_SW_L_POS_BOT,
         Module::RMChassis::SET_MODE_ROTOR
       },
-      Component::CMD::EventMapItem{
-        Device::AI::AI_AUTOPATROL,
-        Module::RMChassis::SET_MODE_ROTOR
-      },
-      Component::CMD::EventMapItem{
-        Device::AI::AI_TURN,
+       Component::CMD::EventMapItem{
+        Device::AI::AIControlData::AI_ROTOR,
         Module::RMChassis::SET_MODE_ROTOR
       }
     },
@@ -60,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,
@@ -93,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,
@@ -109,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,
@@ -175,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,
@@ -187,12 +175,12 @@ Robot::Sentry::Param param = {
 
         .position = {
           /* GIMBAL_CTRL_YAW_ANGLE_IDX */
-          .k = 8.0f,
+          .k = 25.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,
         },
@@ -201,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,
@@ -216,7 +204,7 @@ Robot::Sentry::Param param = {
 
         .position = {
           /* GIMBAL_CTRL_PIT_ANGLE_IDX */
-          .k = 10.0f,
+          .k = 25.0f,
           .p = 1.0f,
           .i = 0.0f,
           .d = 0.0f,
@@ -230,7 +218,6 @@ Robot::Sentry::Param param = {
 
         .out_cutoff_freq = -1.0f,
     },
-
     .yaw_motor = {
       .id_feedback = 0x206,
       .id_control = GM6020_CTRL_ID_BASE,
@@ -246,18 +233,20 @@ Robot::Sentry::Param param = {
     },
 
     .mech_zero = {
-      .yaw = 5.55f,
-      .pit = 0.50f,
+      .yaw = 1.58f,
+      .pit = 4.6f,
       .rol = 0.0f,
     },
-
-    .patrol_range = 0.4f,
-    .patrol_omega = 2.0f,
-    .patrol_hight = 6.0,
+    .scanf_mode{
+      .scanf_yaw_rate = 0.0025f,
+      .scanf_pit_center = 0.04f,
+      .scanf_pit_range = 0.22f,
+      .scanf_pit_omega = 5.0f,
+    },
 
     .limit = {
-      .pitch_max = 0.60f,
-      .pitch_min = 0.19f,
+      .pitch_max = 4.9f,
+      .pitch_min = 4.46f,
     },
 
     .EVENT_MAP = {
@@ -270,24 +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
       }
     },
 
@@ -308,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,
@@ -370,10 +347,11 @@ 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,
+        .reverse = true,
       }
     },
 
@@ -393,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
       },
@@ -402,28 +380,24 @@ 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::AI::AIControlData::AI_AUTOPATROL,
-        Module::Launcher::CHANGE_FIRE_MODE_RELAX
-      },
-        Component::CMD::EventMapItem{
         Device::AI::AIControlData::AI_FIRE_COMMAND,
-        Module::Launcher::LAUNCHER_START_FIRE
+        Module::Launcher::CHANGE_TRIG_MODE_BURST
       },
       Component::CMD::EventMapItem{
-        Device::AI::AIControlData::AI_FIRE_COMMAND,
-        Module::Launcher::CHANGE_TRIG_MODE_SINGLE
+        Device::AI::AIControlData::AI_STOP_FIRE,
+        Module::Launcher::LAUNCHER_STOP_TRIG
       }
 
     },
@@ -443,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,
@@ -505,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,
@@ -519,19 +493,19 @@ 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,
       },
     },
 
-    .EVENT_MAP = {
-     Component::CMD::EventMapItem{
+     .EVENT_MAP = {
+       Component::CMD::EventMapItem{
         Component::CMD::CMD_EVENT_LOST_CTRL,
         Module::Launcher::CHANGE_FIRE_MODE_RELAX
       },
@@ -540,28 +514,24 @@ 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::AI::AIControlData::AI_AUTOPATROL,
-        Module::Launcher::CHANGE_FIRE_MODE_RELAX
-      },
-        Component::CMD::EventMapItem{
         Device::AI::AIControlData::AI_FIRE_COMMAND,
-        Module::Launcher::LAUNCHER_START_FIRE
+        Module::Launcher::CHANGE_TRIG_MODE_BURST
       },
       Component::CMD::EventMapItem{
-        Device::AI::AIControlData::AI_FIRE_COMMAND,
-        Module::Launcher::CHANGE_TRIG_MODE_SINGLE
+        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 0694c30195320030e130184c96bcd19d2132026a..95e1b8cb023a99ac83895db9e2a467b00f5746cd 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_;
@@ -40,7 +41,8 @@ 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),
+        ai_(false),
         bmi088_(param.bmi088_rot),
         cap_(param.cap),
         chassis_(param.chassis, control_freq),