diff --git a/hw/bsp/c-mini/config/infantry.config b/hw/bsp/c-mini/config/infantry.config
deleted file mode 100644
index 96e18272..00000000
--- a/hw/bsp/c-mini/config/infantry.config
+++ /dev/null
@@ -1,104 +0,0 @@
-# CONFIG_auto_generated_config_prefix_board-esp32-c3 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-rm-c=y
-# CONFIG_auto_generated_config_prefix_board-Webots is not set
-# CONFIG_auto_generated_config_prefix_system-Linux_Webots is not set
-CONFIG_auto_generated_config_prefix_system-FreeRTOS=y
-# CONFIG_auto_generated_config_prefix_system-Linux is not set
-CONFIG_INIT_TASK_STACK_DEPTH=2048
-
-#
-# FreeRTOS
-#
-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-balance_infantry is not set
-CONFIG_auto_generated_config_prefix_robot-infantry=y
-# CONFIG_auto_generated_config_prefix_robot-sim_mecanum is not set
-# CONFIG_auto_generated_config_prefix_robot-blink is not set
-# CONFIG_auto_generated_config_prefix_robot-sim_balance is not set
-# CONFIG_auto_generated_config_prefix_robot-wearlab_imu is not set
-
-#
-# 设备
-#
-CONFIG_auto_generated_config_prefix_device-servo=y
-# 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-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-led_rgb=y
-CONFIG_DEVICE_LED_RGB_TASK_STACK_DEPTH=128
-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-laser 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-bmi088=y
-CONFIG_DEVICE_BMI088_TASK_STACK_DEPTH=256
-# CONFIG_auto_generated_config_prefix_device-buzzer is not set
-# CONFIG_auto_generated_config_prefix_device-tof is not set
-# CONFIG_auto_generated_config_prefix_device-blink_led is not set
-CONFIG_auto_generated_config_prefix_device-dr16=y
-CONFIG_DEVICE_DR16_TASK_STACK_DEPTH=384
-CONFIG_auto_generated_config_prefix_device-ahrs=y
-CONFIG_DEVICE_AHRS_TASK_STACK_DEPTH=256
-CONFIG_auto_generated_config_prefix_device-ai=y
-CONFIG_DEVICE_AI_TASK_STACK_DEPTH=384
-
-#
-# 上位机
-#
-# CONFIG_HOST_CTRL_PRIORITY is not set
-# end of 上位机
-
-# CONFIG_auto_generated_config_prefix_device-wearlab is not set
-CONFIG_auto_generated_config_prefix_device-imu=y
-CONFIG_DEVICE_CAN_IMU_TASK_STACK_DEPTH=256
-# end of 设备
-
-#
-# 模块
-#
-CONFIG_auto_generated_config_prefix_module-gimbal=y
-CONFIG_MODULE_GIMBAL_TASK_STACK_DEPTH=512
-# CONFIG_auto_generated_config_prefix_module-balance is not set
-CONFIG_auto_generated_config_prefix_module-chassis=y
-CONFIG_MODULE_CHASSIS_TASK_STACK_DEPTH=384
-CONFIG_auto_generated_config_prefix_module-launcher=y
-CONFIG_MODULE_LAUNCHER_TASK_STACK_DEPTH=384
-# CONFIG_auto_generated_config_prefix_module-can_imu is not set
-# CONFIG_auto_generated_config_prefix_module-wheel_leg is not set
-# end of 模块
diff --git a/hw/bsp/rm-c/config/helm_infantry.config b/hw/bsp/rm-c/config/helm_infantry.config
index 1f4348f3..0d5f0a96 100644
--- a/hw/bsp/rm-c/config/helm_infantry.config
+++ b/hw/bsp/rm-c/config/helm_infantry.config
@@ -1,50 +1,42 @@
-# CONFIG_auto_generated_config_prefix_board-c-mini is not set
-# CONFIG_auto_generated_config_prefix_board-dual_canfd 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-f103_can is not set
-# CONFIG_auto_generated_config_prefix_board-mangopi_r818 is not set
-# CONFIG_auto_generated_config_prefix_board-microswitch is not set
-# CONFIG_auto_generated_config_prefix_board-MiniPC is not set
-# CONFIG_auto_generated_config_prefix_board-MiniPC_with_canfd is not set
-# CONFIG_auto_generated_config_prefix_board-node_imu is not set
 CONFIG_auto_generated_config_prefix_board-rm-c=y
+# CONFIG_auto_generated_config_prefix_board-MiniPC is not set
 # CONFIG_auto_generated_config_prefix_board-Webots is not set
-# CONFIG_auto_generated_config_prefix_system-Bootloader is not set
-CONFIG_auto_generated_config_prefix_system-FreeRTOS=y
+# CONFIG_auto_generated_config_prefix_board-f103_can 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-esp32-c3-arduino is not set
+# CONFIG_auto_generated_config_prefix_board-esp32-s3-arduino is not set
+# CONFIG_auto_generated_config_prefix_board-atom is not set
+# CONFIG_auto_generated_config_prefix_board-dual_canfd is not set
+# CONFIG_auto_generated_config_prefix_system-None is not set
 # CONFIG_auto_generated_config_prefix_system-Linux is not set
 # CONFIG_auto_generated_config_prefix_system-Linux_Webots is not set
-# 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-FreeRTOS=y
+CONFIG_INIT_TASK_STACK_DEPTH=2048
 
 #
 # 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-balance_infantry is not set
+# CONFIG_auto_generated_config_prefix_robot-omni_infantry is not set
+# CONFIG_auto_generated_config_prefix_robot-udp_to_uart is not set
+# CONFIG_auto_generated_config_prefix_robot-arm_engineer is not set
 # CONFIG_auto_generated_config_prefix_robot-ble_net_config is not set
-# CONFIG_auto_generated_config_prefix_robot-blink is not set
-# CONFIG_auto_generated_config_prefix_robot-bootloader is not set
-# CONFIG_auto_generated_config_prefix_robot-canfd_to_uart is not set
-# CONFIG_auto_generated_config_prefix_robot-can_to_uart is not set
-# CONFIG_auto_generated_config_prefix_robot-custom_controller is not set
-# CONFIG_auto_generated_config_prefix_robot-dart is not set
 # CONFIG_auto_generated_config_prefix_robot-drone_gimbal is not set
-# CONFIG_auto_generated_config_prefix_robot-engineer is not set
-CONFIG_auto_generated_config_prefix_robot-helm_infantry=y
+# CONFIG_auto_generated_config_prefix_robot-sentry is not set
+# CONFIG_auto_generated_config_prefix_robot-dart is not set
 # CONFIG_auto_generated_config_prefix_robot-hero is not set
 # CONFIG_auto_generated_config_prefix_robot-infantry is not set
-# CONFIG_auto_generated_config_prefix_robot-microswitch is not set
-# CONFIG_auto_generated_config_prefix_robot-sentry is not set
-# CONFIG_auto_generated_config_prefix_robot-sim_balance is not set
-# CONFIG_auto_generated_config_prefix_robot-sim_mecanum is not set
 # CONFIG_auto_generated_config_prefix_robot-uart_net_config is not set
-# CONFIG_auto_generated_config_prefix_robot-udp_to_uart is not set
-# CONFIG_auto_generated_config_prefix_robot-wearlab_imu is not set
+# CONFIG_auto_generated_config_prefix_robot-canfd_imu is not set
+CONFIG_auto_generated_config_prefix_robot-helm_infantry=y
+# CONFIG_auto_generated_config_prefix_robot-sim_mecanum is not set
+# CONFIG_auto_generated_config_prefix_robot-blink is not set
 
 #
 # 组件
@@ -53,25 +45,58 @@ CONFIG_auto_generated_config_prefix_robot-helm_infantry=y
 #
 # 设备
 #
-CONFIG_auto_generated_config_prefix_device-ahrs=y
-CONFIG_DEVICE_AHRS_TASK_STACK_DEPTH=256
+# CONFIG_auto_generated_config_prefix_device-laser 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-motor=y
+CONFIG_auto_generated_config_prefix_device-can=y
+CONFIG_auto_generated_config_prefix_device-bmi088=y
+CONFIG_DEVICE_BMI088_TASK_STACK_DEPTH=256
+# CONFIG_auto_generated_config_prefix_device-net_config is not set
 # CONFIG_auto_generated_config_prefix_device-ahrs-9 is not set
-CONFIG_auto_generated_config_prefix_device-ai=y
+CONFIG_DEVICE_AHRS_TASK_STACK_DEPTH=256
+# CONFIG_auto_generated_config_prefix_device-blink_led is not set
+# 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-mmc5603 is not set
+CONFIG_auto_generated_config_prefix_device-servo=y
+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-ai is not set
 CONFIG_DEVICE_AI_TASK_STACK_DEPTH=384
+CONFIG_HOST_CTRL_PRIORITY=y
+CONFIG_auto_generated_config_prefix_device-aim=y
 
 #
 # 上位机
 #
-# CONFIG_HOST_CTRL_PRIORITY is not set
 # end of 上位机
 
-# CONFIG_auto_generated_config_prefix_device-blink_led is not set
-CONFIG_auto_generated_config_prefix_device-bmi088=y
-CONFIG_DEVICE_BMI088_TASK_STACK_DEPTH=256
-# CONFIG_auto_generated_config_prefix_device-bq27220 is not set
+# CONFIG_auto_generated_config_prefix_device-mech is not set
+# CONFIG_auto_generated_config_prefix_device-mt6701 is not set
 # CONFIG_auto_generated_config_prefix_device-buzzer is not set
-CONFIG_auto_generated_config_prefix_device-can=y
+# CONFIG_auto_generated_config_prefix_device-ina226 is not set
 # CONFIG_auto_generated_config_prefix_device-canfd is not set
+CONFIG_auto_generated_config_prefix_device-ahrs=y
 CONFIG_auto_generated_config_prefix_device-cap=y
 CONFIG_DEVICE_CAP_TASK_STACK_DEPTH=256
 
@@ -83,73 +108,28 @@ CONFIG_CAP_PERCENT_WORK=30
 CONFIG_CAP_MAX_LOAD=100
 # end of 超级电容
 
-# CONFIG_auto_generated_config_prefix_device-custom_controller is not set
 CONFIG_auto_generated_config_prefix_device-dr16=y
 CONFIG_DEVICE_DR16_TASK_STACK_DEPTH=384
 # CONFIG_auto_generated_config_prefix_device-icm42688 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-ina226 is not set
-# CONFIG_auto_generated_config_prefix_device-laser 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-mech is not set
-# CONFIG_auto_generated_config_prefix_device-microswitch is not set
-# CONFIG_auto_generated_config_prefix_device-mmc5603 is not set
-CONFIG_auto_generated_config_prefix_device-motor=y
-# 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
-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
-# end of 裁判系统
-
-#
-# 操作手UI
-#
-CONFIG_UI_DYNAMIC_CYCLE=20
-CONFIG_UI_STATIC_CYCLE=1000
-# end of 操作手UI
-
-CONFIG_auto_generated_config_prefix_device-servo=y
-# CONFIG_auto_generated_config_prefix_device-simulator is not set
-# CONFIG_auto_generated_config_prefix_device-spl06_001 is not set
-# CONFIG_auto_generated_config_prefix_device-tof is not set
 # end of 设备
 
 #
 # 模块
 #
-# CONFIG_auto_generated_config_prefix_module-balance is not set
-# CONFIG_auto_generated_config_prefix_module-ble_net_config is not set
-# CONFIG_auto_generated_config_prefix_module-canfd_to_uart is not set
-# CONFIG_auto_generated_config_prefix_module-can_imu is not set
-# CONFIG_auto_generated_config_prefix_module-can_imu_wearlab is not set
-# CONFIG_auto_generated_config_prefix_module-can_usart is not set
-# CONFIG_auto_generated_config_prefix_module-chassis is not set
-# CONFIG_auto_generated_config_prefix_module-custom_controller is not set
 # CONFIG_auto_generated_config_prefix_module-dart_gimbal is not set
-# CONFIG_auto_generated_config_prefix_module-dart_launcher is not set
-# CONFIG_auto_generated_config_prefix_module-engineer_chassis is not set
-CONFIG_auto_generated_config_prefix_module-gimbal=y
-CONFIG_MODULE_GIMBAL_TASK_STACK_DEPTH=512
+# CONFIG_auto_generated_config_prefix_module-chassis is not set
+# CONFIG_auto_generated_config_prefix_module-ble_net_config is not set
+# CONFIG_auto_generated_config_prefix_module-omni_chassis is not set
+# CONFIG_auto_generated_config_prefix_module-robot_arm is not set
 CONFIG_auto_generated_config_prefix_module-helm_chassis=y
 CONFIG_MODULE_LAUNCHER_TASK_STACK_DEPTH=384
+CONFIG_auto_generated_config_prefix_module-gimbal=y
+CONFIG_MODULE_GIMBAL_TASK_STACK_DEPTH=512
+# CONFIG_auto_generated_config_prefix_module-topic_share_uart is not set
+# CONFIG_auto_generated_config_prefix_module-dart_launcher is not set
+# CONFIG_auto_generated_config_prefix_module-performance is not set
 CONFIG_auto_generated_config_prefix_module-launcher=y
+# CONFIG_auto_generated_config_prefix_module-engineer_chassis is not set
+# CONFIG_auto_generated_config_prefix_module-canfd_imu is not set
 # CONFIG_auto_generated_config_prefix_module-launcher_drone is not set
-# CONFIG_auto_generated_config_prefix_module-microswitch is not set
-# CONFIG_auto_generated_config_prefix_module-ore_collect is not set
-# CONFIG_auto_generated_config_prefix_module-performance is not set
-# CONFIG_auto_generated_config_prefix_module-topic_share_uart is not set
-# CONFIG_auto_generated_config_prefix_module-uart_update is not set
-# CONFIG_auto_generated_config_prefix_module-wheel_leg is not set
 # end of 模块
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..4704de27
--- /dev/null
+++ b/hw/bsp/rm-c/config/omni_infantry.config
@@ -0,0 +1,135 @@
+CONFIG_auto_generated_config_prefix_board-rm-c=y
+# CONFIG_auto_generated_config_prefix_board-MiniPC is not set
+# CONFIG_auto_generated_config_prefix_board-Webots is not set
+# CONFIG_auto_generated_config_prefix_board-f103_can 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-esp32-c3-arduino is not set
+# CONFIG_auto_generated_config_prefix_board-esp32-s3-arduino is not set
+# CONFIG_auto_generated_config_prefix_board-atom is not set
+# CONFIG_auto_generated_config_prefix_board-dual_canfd is not set
+# CONFIG_auto_generated_config_prefix_system-None is not set
+# CONFIG_auto_generated_config_prefix_system-Linux is not set
+# CONFIG_auto_generated_config_prefix_system-Linux_Webots is not set
+# CONFIG_auto_generated_config_prefix_system-Bootloader is not set
+CONFIG_auto_generated_config_prefix_system-FreeRTOS=y
+CONFIG_INIT_TASK_STACK_DEPTH=2048
+
+#
+# FreeRTOS
+#
+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-omni_infantry=y
+# CONFIG_auto_generated_config_prefix_robot-udp_to_uart is not set
+# CONFIG_auto_generated_config_prefix_robot-arm_engineer is not set
+# CONFIG_auto_generated_config_prefix_robot-ble_net_config is not set
+# CONFIG_auto_generated_config_prefix_robot-drone_gimbal is not set
+# CONFIG_auto_generated_config_prefix_robot-sentry is not set
+# CONFIG_auto_generated_config_prefix_robot-dart is not set
+# CONFIG_auto_generated_config_prefix_robot-hero is not set
+# CONFIG_auto_generated_config_prefix_robot-infantry is not set
+# CONFIG_auto_generated_config_prefix_robot-uart_net_config is not set
+# CONFIG_auto_generated_config_prefix_robot-canfd_imu is not set
+# CONFIG_auto_generated_config_prefix_robot-helm_infantry is not set
+# CONFIG_auto_generated_config_prefix_robot-sim_mecanum is not set
+# CONFIG_auto_generated_config_prefix_robot-blink is not set
+
+#
+# 组件
+#
+
+#
+# 设备
+#
+# CONFIG_auto_generated_config_prefix_device-laser 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-motor=y
+CONFIG_auto_generated_config_prefix_device-can=y
+CONFIG_auto_generated_config_prefix_device-bmi088=y
+CONFIG_DEVICE_BMI088_TASK_STACK_DEPTH=256
+# CONFIG_auto_generated_config_prefix_device-net_config is not set
+# CONFIG_auto_generated_config_prefix_device-ahrs-9 is not set
+CONFIG_DEVICE_AHRS_TASK_STACK_DEPTH=256
+# CONFIG_auto_generated_config_prefix_device-blink_led is not set
+# 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-mmc5603 is not set
+CONFIG_auto_generated_config_prefix_device-servo=y
+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-ai is not set
+CONFIG_DEVICE_AI_TASK_STACK_DEPTH=384
+CONFIG_HOST_CTRL_PRIORITY=y
+CONFIG_auto_generated_config_prefix_device-aim=y
+
+#
+# 上位机
+#
+# end of 上位机
+
+# CONFIG_auto_generated_config_prefix_device-mech is not set
+# CONFIG_auto_generated_config_prefix_device-mt6701 is not set
+# CONFIG_auto_generated_config_prefix_device-buzzer is not set
+# CONFIG_auto_generated_config_prefix_device-ina226 is not set
+# CONFIG_auto_generated_config_prefix_device-canfd is not set
+CONFIG_auto_generated_config_prefix_device-ahrs=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-dr16=y
+CONFIG_DEVICE_DR16_TASK_STACK_DEPTH=384
+# CONFIG_auto_generated_config_prefix_device-icm42688 is not set
+# end of 设备
+
+#
+# 模块
+#
+# CONFIG_auto_generated_config_prefix_module-dart_gimbal is not set
+# CONFIG_auto_generated_config_prefix_module-chassis is not set
+# CONFIG_auto_generated_config_prefix_module-ble_net_config is not set
+CONFIG_auto_generated_config_prefix_module-omni_chassis=y
+# CONFIG_auto_generated_config_prefix_module-robot_arm is not set
+# CONFIG_auto_generated_config_prefix_module-helm_chassis is not set
+CONFIG_MODULE_LAUNCHER_TASK_STACK_DEPTH=384
+CONFIG_auto_generated_config_prefix_module-gimbal=y
+CONFIG_MODULE_GIMBAL_TASK_STACK_DEPTH=512
+# CONFIG_auto_generated_config_prefix_module-topic_share_uart is not set
+# CONFIG_auto_generated_config_prefix_module-dart_launcher is not set
+# CONFIG_auto_generated_config_prefix_module-performance is not set
+CONFIG_auto_generated_config_prefix_module-launcher=y
+# CONFIG_auto_generated_config_prefix_module-engineer_chassis is not set
+# CONFIG_auto_generated_config_prefix_module-canfd_imu is not set
+# CONFIG_auto_generated_config_prefix_module-launcher_drone is not set
+# end of 模块
diff --git a/src/component/comp_actuator.hpp b/src/component/comp_actuator.hpp
index 33f7157d..4415befa 100644
--- a/src/component/comp_actuator.hpp
+++ b/src/component/comp_actuator.hpp
@@ -79,6 +79,8 @@ class PosActuator {
 
   float SpeedCalculate(float setpoint, float feedback, float dt);
 
+  float GetLastError() { return pid_speed_.GetError(); }
+
   void Reset();
 
  private:
diff --git a/src/component/comp_pid.hpp b/src/component/comp_pid.hpp
index 5cef0560..49111dde 100644
--- a/src/component/comp_pid.hpp
+++ b/src/component/comp_pid.hpp
@@ -40,6 +40,8 @@ class PID {
 
   float Calculate(float sp, float fb, float fb_dot, float dt);
 
+  float GetError() { return last_.err; }
+
  private:
   Param param_;
 
diff --git a/src/device/aim/Kconfig b/src/device/aim/Kconfig
new file mode 100644
index 00000000..41f203e5
--- /dev/null
+++ b/src/device/aim/Kconfig
@@ -0,0 +1,15 @@
+config DEVICE_AI_TASK_STACK_DEPTH
+
+    int "AI任务堆栈大小"
+
+    range 128 4096
+
+    default 256
+
+    menu "上位机"
+
+    config HOST_CTRL_PRIORITY
+
+        tristate "优先把控制权交给上位机"
+
+    endmenu
diff --git a/src/device/aim/dev_aim.cpp b/src/device/aim/dev_aim.cpp
new file mode 100644
index 00000000..4fcab6d3
--- /dev/null
+++ b/src/device/aim/dev_aim.cpp
@@ -0,0 +1,150 @@
+#include "dev_aim.hpp"
+using namespace Device;
+
+#define AI_LEN_RX_BUFF (sizeof(AIM::RefForAI_t))
+#define AI_LEN_TX_BUFF (sizeof(AIM::TranToAI_t))
+
+static uint8_t rxbuf[AI_LEN_RX_BUFF];
+static uint8_t txbuf[AI_LEN_TX_BUFF];
+
+/* clang-format off */
+AIM::AIM()
+    : event_(Message::Event::FindEvent("cmd_event")),
+      cmd_tp_("cmd_ai"),
+      data_ready_(false)
+{
+
+  auto rx_cplt_callback = [](void *arg) {
+    AIM *aim = static_cast<AIM *>(arg);
+    aim->data_ready_.Post();
+  };
+
+  bsp_uart_register_callback(BSP_UART_AI, BSP_UART_RX_CPLT_CB, rx_cplt_callback,
+                             this);
+
+  Component::CMD::RegisterController(this->cmd_tp_);
+ auto ai_thread = [](AIM *aim) {
+
+    auto eulr_sub = Message::Subscriber<Component::Type::Eulr>("imu_eulr");
+
+     while (1)
+     {
+        /* 接收imu和裁判系统数据 */
+        eulr_sub.DumpData(aim->eulr_);
+
+        /* 接收上位机数据 */
+        aim->StartRecv();
+        if (aim->data_ready_.Wait(0))
+        {
+            aim->PraseHost();
+            aim->PackCMD();
+        }
+
+        //发送数据给上位机
+        aim->PackMCU();
+        aim->StartTran();
+
+        System::Thread::Sleep(2);
+     }
+    };
+    this->thread_.Create(ai_thread, this, "aim_thread", DEVICE_AI_TASK_STACK_DEPTH,
+                       System::Thread::REALTIME);
+}
+
+double convert_to_0_to_2pi(double theta_prime) {
+  double theta = fmod(theta_prime + 2 * M_PI, 2 * M_PI);
+  return theta;
+}
+
+bool AIM::PackCMD() {
+  /* 确保遥控器开关最高控制权,关遥控器即断控 */
+  if (!Component::CMD::Online()) {
+    return false;
+  }
+
+    if(from_host_.yaw == 0 && from_host_.pitch == 0)
+    {
+      this->to_cmd_.yaw = this->eulr_.yaw;
+      this->to_cmd_.pitch = this->eulr_.pit;
+      this->to_cmd_.roll = this->eulr_.rol;
+      return 0;
+    }
+
+  /* 控制源:AI */
+  if (Component::CMD::GetCtrlSource() == Component::CMD::CTRL_SOURCE_AI) {
+    /* OP控制模式,用于鼠标右键自瞄 */
+    if (Component::CMD::GetCtrlMode() == Component::CMD::CMD_OP_CTRL) {
+
+  if(from_host_.header == 0xA5)
+  {
+    this->to_cmd_.yaw = from_host_.yaw;
+    this->to_cmd_.pitch = from_host_.pitch;
+    this->to_cmd_.roll = this->eulr_.rol;
+  }
+
+      memcpy(&(this->cmd_.gimbal.eulr), &(this->to_cmd_),
+             sizeof(this->cmd_.gimbal.eulr));
+
+      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;
+}
+
+bool AIM::PraseHost() {
+   if (Component::CRC16::Verify(rxbuf, sizeof(this->from_host_))) {
+    this->cmd_.online = true;
+    this->last_online_time_ = bsp_time_get_ms();
+    memcpy(&(this->from_host_), rxbuf, sizeof(this->from_host_));
+    memset(rxbuf, 0, AI_LEN_RX_BUFF);
+    return true;
+  }
+  return false;
+}
+
+double convert_to_minus_pi_to_pi(double theta) {
+  while (theta > M_PI) {
+    theta -= 2 * M_PI;
+  }
+  while (theta < -M_PI) {
+    theta += 2 * M_PI;
+  }
+  return theta;
+}
+
+bool AIM::PackMCU()
+{
+    this->to_host_.header = 0x5A;
+    this->to_host_.detect_color = 0;
+    this->to_host_.reset_tracker = 0;
+    this->to_host_.current_v = 24;
+    this->to_host_.yaw = this->eulr_.yaw;
+    this->to_host_.pitch = this->eulr_.pit;
+    this->to_host_.roll = this->eulr_.rol;
+    this->to_host_.aim_x = this->from_host_.x;
+    this->to_host_.aim_y = this->from_host_.y;
+    this->to_host_.aim_z = this->from_host_.z;
+    this->to_host_.checksum = Component::CRC16::Calculate(
+      reinterpret_cast<const uint8_t *>(&(this->to_host_)),
+      sizeof(this->to_host_) - sizeof(uint16_t), CRC16_INIT);
+
+    return true;
+}
+bool AIM::StartTran()
+{
+    size_t len = sizeof(this->to_host_);
+
+    void *src = NULL;
+    src = &(this->to_host_);
+    memcpy(txbuf, src, len);
+    return bsp_uart_transmit(BSP_UART_AI, txbuf, len,false) == BSP_OK;
+}
+
+bool AIM::StartRecv()
+{
+  return bsp_uart_receive(BSP_UART_AI, rxbuf, sizeof(rxbuf), false) == BSP_OK;
+}
diff --git a/src/device/aim/dev_aim.hpp b/src/device/aim/dev_aim.hpp
new file mode 100644
index 00000000..9cd682d5
--- /dev/null
+++ b/src/device/aim/dev_aim.hpp
@@ -0,0 +1,123 @@
+#pragma once
+
+#include "bsp_uart.h"
+#include "comp_cmd.hpp"
+#include "comp_crc16.hpp"
+#include "dev_ahrs.hpp"
+#include "dev_referee.hpp"
+#include "device.hpp"
+
+#ifndef pi
+#define pi 3.1415926535f
+#endif
+
+/* clang-format off */
+namespace Device {
+class AIM {
+ public:
+
+  //MCU需要从上位机获取的数据
+  typedef struct __attribute__((packed)) RevforAI{
+  uint8_t header = 0xA5;
+  bool is_fire : 1;
+  uint8_t reserved : 1;
+  float x;
+  float y;
+  float z;
+  float v_yaw;
+  float pitch;
+  float yaw;
+  uint16_t checksum = 0;
+  }RefForAI_t;
+
+  typedef struct __attribute__ ((packed)) TranToAI{
+  uint8_t header = 0x5A;
+  uint8_t detect_color : 1;  // 0-red 1-blue
+  bool reset_tracker : 1;
+  uint8_t reserved : 6;
+  float current_v; // m/s
+  float yaw;
+  float pitch;
+  float roll;
+  float aim_x;
+  float aim_y;
+  float aim_z;
+  uint16_t checksum = 0;
+  }TranToAI_t;
+
+  typedef struct {
+    uint8_t game_type;
+    Device::Referee::Status status;
+    uint8_t team;
+    uint8_t robot_id;
+    uint8_t robot_buff;
+    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 yaw;
+    float pitch;
+    float roll;
+  }ToCMD_t;
+
+  typedef enum {
+    AI_STOP_FIRE,
+    AI_FIRE_COMMAND,
+  } AIControlData;
+
+  AIControlData aim_status_;
+
+  RefForAI_t from_host_;
+  TranToAI_t to_host_;
+
+/* ----------------------------------------------------------------------------------------------------- */
+  AIM();
+
+  //system
+  Component::Type::Eulr eulr_;
+
+  Component::CMD::Data cmd_{};
+
+  Referee::SentryDecisionData cmd_for_ref_;
+  /* Topic & Event */
+
+  Message::Event event_;
+
+  Message::Topic<Component::CMD::Data> cmd_tp_;
+
+  /* Task Control */
+  System::Thread thread_;
+
+  System::Semaphore data_ready_;
+
+  ToCMD_t to_cmd_;
+
+  bool StartRecv();
+
+  bool StartTran();
+
+  bool PraseHost();
+
+  bool PackCMD();
+
+  bool PackMCU();
+
+uint32_t last_online_time_ = 0;
+};
+}  // namespace Device
diff --git a/src/device/aim/info.cmake b/src/device/aim/info.cmake
new file mode 100644
index 00000000..322c4b19
--- /dev/null
+++ b/src/device/aim/info.cmake
@@ -0,0 +1,6 @@
+CHECK_SUB_ENABLE(MODULE_ENABLE device)
+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/gimbal/mod_gimbal.cpp b/src/module/gimbal/mod_gimbal.cpp
index b0b494c8..9374a268 100644
--- a/src/module/gimbal/mod_gimbal.cpp
+++ b/src/module/gimbal/mod_gimbal.cpp
@@ -15,6 +15,8 @@ Gimbal::Gimbal(Param& param, float control_freq)
       st_(param.st),
       yaw_actuator_(this->param_.yaw_actr, control_freq),
       pit_actuator_(this->param_.pit_actr, control_freq),
+      yaw_ai_actuator_(this->param_.yaw_ai_actr, control_freq),
+      pit_ai_actuator_(this->param_.pit_ai_actr, control_freq),
       yaw_motor_(this->param_.yaw_motor, "Gimbal_Yaw"),
       pit_motor_(this->param_.pit_motor, "Gimbal_Pitch"),
       ctrl_lock_(true) {
@@ -24,14 +26,13 @@ Gimbal::Gimbal(Param& param, float control_freq)
     switch (event) {
       case SET_MODE_RELAX:
       case SET_MODE_ABSOLUTE:
+        Component::CMD::SetCtrlSource(Component::CMD::CTRL_SOURCE_RC);
         gimbal->SetMode(static_cast<Mode>(event));
         break;
 
-      case START_AUTO_AIM:
+      case SET_MODE_AUTO_AIM:
         Component::CMD::SetCtrlSource(Component::CMD::CTRL_SOURCE_AI);
-        break;
-      case STOP_AUTO_AIM:
-        Component::CMD::SetCtrlSource(Component::CMD::CTRL_SOURCE_RC);
+        gimbal->SetMode((static_cast<Mode>(event)));
         break;
     }
     gimbal->ctrl_lock_.Post();
@@ -61,6 +62,10 @@ Gimbal::Gimbal(Param& param, float control_freq)
       gimbal->ctrl_lock_.Post();
 
       gimbal->yaw_tp_.Publish(gimbal->yaw_);
+      gimbal->pit_tp_.Publish(gimbal->pit_);
+      gimbal->alpha_tp_.Publish(gimbal->alpha_);
+      gimbal->eulr_yaw1_tp_.Publish(gimbal->eulr_yaw1_);
+      gimbal->tan_pit_tp_.Publish(gimbal->tan_pit_);
 
       /* 运行结束,等待下一次唤醒 */
       gimbal->thread_.SleepUntil(2, last_online_time);
@@ -80,6 +85,91 @@ void Gimbal::UpdateFeedback() {
   this->yaw_motor_.Update();
 
   this->yaw_ = this->yaw_motor_.GetAngle() - this->param_.mech_zero.yaw;
+  this->eulr_yaw1_ = eulr_.yaw;
+  this->pit_ = atan(
+      sqrt(tan(ChangeAngleRange(eulr_.pit) -
+               (this->pit_motor_.GetAngle() - this->param_.mech_zero.pit)) *
+               tan(ChangeAngleRange(eulr_.pit) -
+                   (this->pit_motor_.GetAngle() - this->param_.mech_zero.pit)) +
+           tan(eulr_.rol) * tan(eulr_.rol)));
+  this->alpha_ = this->GetAlpha();
+  this->slope_angle_ = this->pit_ * 180 / M_PI;
+  this->yaw_motor_value_ = this->yaw_motor_.GetAngle().Value();
+  this->pit_motor_value_ = this->pit_motor_.GetAngle().Value();
+
+  this->tan_pit_ =
+      tan(ChangeAngleRange(eulr_.pit) -
+          (this->pit_motor_.GetAngle() - this->param_.mech_zero.pit));
+  this->tan_rol_ = tan(ChangeAngleRange(eulr_.rol));
+  this->test_angle_3_ = ChangeAngleRange(eulr_.pit);
+  // this->tan_yaw_ = -(this->yaw_motor_.GetAngle().Value() -
+  // this->param_.mech_zero.yaw) +
+  //  ChangeAngleRange(eulr_.yaw);
+  this->test_angle_4_ = RotateVector3D(static_cast<float>(this->test_angle_3_),
+                                       static_cast<float>(this->tan_pit_),
+                                       static_cast<float>(this->tan_rol_));
+}
+
+double Gimbal::GetAlpha() {
+  if (this->tan_rol_ > 0 && this->tan_pit_ > 0) {
+    this->alpha_ = ((this->tan_rol_ * this->tan_rol_) /
+                    (tan(this->pit_) * tan(this->pit_))) *
+                   M_PI_2;
+  } else if (this->tan_rol_ > 0 && this->tan_pit_ < 0) {
+    this->alpha_ =
+        M_PI -
+        ((tan_rol_ * tan_rol_) / (tan(this->pit_) * tan(this->pit_))) * M_PI_2;
+  } else if (this->tan_rol_ < 0 && this->tan_pit_ < 0) {
+    this->alpha_ =
+        M_PI +
+        ((tan_rol_ * tan_rol_) / (tan(this->pit_) * tan(this->pit_))) * M_PI_2;
+  } else if (this->tan_rol_ < 0 && this->tan_pit_ > 0) {
+    this->alpha_ =
+        2 * M_PI -
+        ((tan_rol_ * tan_rol_) / (tan(this->pit_) * tan(this->pit_))) * M_PI_2;
+  }
+  return this->alpha_;
+}
+
+float Gimbal::ChangeAngleRange(float angle) {
+  angle = fmod(angle, 2 * M_PI);
+  if (angle < 0) {
+    angle += 2 * M_PI;
+  }
+  // 将角度从 [0, 2π] 转换到 [-π, π]
+  if (angle > M_PI) {
+    angle -= 2 * M_PI;
+  }
+
+  return angle;
+}
+
+float Gimbal::RotateVector3D(float x, float y, float z) {
+  float cos_x = cosf(x);
+  float sin_x = sinf(x);
+  float cos_y = cosf(y);
+  float sin_y = sinf(y);
+  float cos_z = cosf(z);
+  float sin_z = sinf(z);
+  float angle = 0.0f;
+  rotation_mat_[0][0] = cos_x * cos_y;
+  rotation_mat_[0][1] = -sin_x * cos_z + cos_x * sin_y * sin_z;
+  rotation_mat_[0][2] = sin_x * cos_z + cos_x * sin_y * cos_z;
+  rotation_mat_[1][0] = sin_x * cos_y;
+  rotation_mat_[1][1] = cos_x * cos_z + sin_x * sin_y * sin_z;
+  rotation_mat_[1][2] = -cos_x * sin_z + sin_x * sin_y * cos_z;
+  rotation_mat_[2][0] = -sin_y;
+  rotation_mat_[2][1] = cos_y * sin_z;
+  rotation_mat_[2][2] = cos_y * cos_z;
+  angle = atan(-sqrt(rotation_mat_[0][2] * rotation_mat_[0][2] +
+                     rotation_mat_[1][2] * rotation_mat_[1][2]) /
+               rotation_mat_[2][2]);
+  return angle;
+  // return {{{cos_x * cos_y, -sin_x * cos_z + cos_x * sin_y * sin_z,
+  //           sin_x * cos_z + cos_x * sin_y * cos_z},
+  //          {sin_x * cos_y, cos_x * cos_z + sin_x * sin_y * sin_z,
+  //           -cos_x * sin_z + sin_x * sin_y * cos_z},
+  //          {-sin_y, cos_y * sin_z, cos_y * cos_z}}};
 }
 
 void Gimbal::Control() {
@@ -95,7 +185,7 @@ void Gimbal::Control() {
   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;
-
+    pit_ = gimbal_pit_cmd;
   } else {
     gimbal_yaw_cmd = Component::Type::CycleValue(this->cmd_.eulr.yaw) -
                      this->setpoint_.eulr_.yaw;
@@ -148,6 +238,16 @@ void Gimbal::Control() {
       this->yaw_motor_.Control(yaw_out);
       this->pit_motor_.Control(pit_out);
 
+      break;
+    case AI_CONTROL:
+      yaw_out = this->yaw_ai_actuator_.Calculate(
+          this->setpoint_.eulr_.yaw, this->gyro_.z, this->eulr_.yaw, this->dt_);
+
+      pit_out = this->pit_ai_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;
   }
 }
@@ -163,11 +263,22 @@ void Gimbal::SetMode(Mode mode) {
 
   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->setpoint_.eulr_.yaw = this->eulr_.yaw;
+  // if (this->mode_ == RELAX) {
+  //   if (mode == ABSOLUTE) {
+  //     this->setpoint_.eulr_.yaw = this->eulr_.yaw;
+  //   }
+  // }
+  // if (this->mode_ == ABSOLUTE) {
+  //   if (mode == AI_CONTROL) {
+  //     this->setpoint_.eulr_.yaw = this->eulr_.yaw;
+  //   }
+  // }
+  // if (this->mode_ == AI_CONTROL) {
+  //   if (mode == ABSOLUTE) {
+  //     this->setpoint_.eulr_.yaw = this->eulr_.yaw;
+  //   }
+  // }
   this->mode_ = mode;
 }
 
diff --git a/src/module/gimbal/mod_gimbal.hpp b/src/module/gimbal/mod_gimbal.hpp
index 378c063d..80a4a20d 100644
--- a/src/module/gimbal/mod_gimbal.hpp
+++ b/src/module/gimbal/mod_gimbal.hpp
@@ -20,7 +20,9 @@ class Gimbal {
   /* 云台运行模式 */
   typedef enum {
     RELAX, /* 放松模式,电机不输出。一般情况云台初始化之后的模式 */
-    ABSOLUTE, /* 绝对坐标系控制,控制在空间内的绝对姿态 */
+    ABSOLUTE,  /* 绝对坐标系控制,控制在空间内的绝对姿态 */
+    AI_CONTROL /*Host control mode, based on absolute coordinate system
+                  control*/
   } Mode;
 
   enum {
@@ -34,8 +36,7 @@ class Gimbal {
   typedef enum {
     SET_MODE_RELAX,
     SET_MODE_ABSOLUTE,
-    START_AUTO_AIM,
-    STOP_AUTO_AIM,
+    SET_MODE_AUTO_AIM,
   } GimbalEvent;
 
   typedef struct {
@@ -44,6 +45,8 @@ class Gimbal {
 
     Component::PosActuator::Param yaw_actr;
     Component::PosActuator::Param pit_actr;
+    Component::PosActuator::Param yaw_ai_actr;
+    Component::PosActuator::Param pit_ai_actr;
 
     Device::RMMotor::Param yaw_motor;
     Device::RMMotor::Param pit_motor;
@@ -67,12 +70,20 @@ class Gimbal {
 
   void Control();
 
+  float ChangeAngleRange(float angle);
+
   void SetMode(Mode mode);
 
   static void DrawUIStatic(Gimbal *gimbal);
 
   static void DrawUIDynamic(Gimbal *gimbal);
 
+  void GetSlopeAngle();
+
+  float RotateVector3D(float x, float y, float z);
+
+  double GetAlpha();
+
  private:
   uint64_t last_wakeup_ = 0;
 
@@ -80,6 +91,9 @@ class Gimbal {
 
   float dt_ = 0.0f;
 
+  float yaw_motor_value_;
+  float pit_motor_value_;
+
   Param param_;
 
   Gimbal::Mode mode_ = RELAX; /* 云台模式 */
@@ -92,6 +106,8 @@ class Gimbal {
 
   Component::PosActuator yaw_actuator_;
   Component::PosActuator pit_actuator_;
+  Component::PosActuator yaw_ai_actuator_;
+  Component::PosActuator pit_ai_actuator_;
 
   Device::RMMotor yaw_motor_;
   Device::RMMotor pit_motor_;
@@ -101,8 +117,27 @@ class Gimbal {
   System::Semaphore ctrl_lock_;
 
   Message::Topic<float> yaw_tp_ = Message::Topic<float>("chassis_yaw");
+  Message::Topic<float> eulr_tp_ = Message::Topic<float>("ahrs_eulr");
+  Message::Topic<float> quat_tp_ = Message::Topic<float>("ahrs_quat");
+  Message::Topic<float> pit_tp_ = Message::Topic<float>("chassis_pitch");
+  Message::Topic<double> alpha_tp_ = Message::Topic<double>("chassis_alpha");
+  Message::Topic<float> eulr_yaw1_tp_ =
+      Message::Topic<float>("chassis_eulr_yaw1"); /* 首次云台偏航角 */
+  Message::Topic<double> tan_pit_tp_ =
+      Message::Topic<double>("chassis_tan_pit");
 
   float yaw_;
+  float pit_;
+  double alpha_;
+  double slope_angle_;
+  float eulr_yaw1_;
+  double tan_pit_;
+  // Component::Type::CycleValue test_angle_2_;
+  double tan_rol_;
+  double test_angle_3_;
+  float test_angle_4_;
+
+  float rotation_mat_[3][3];
 
   Component::UI::String string_;
 
diff --git a/src/module/helm_chassis/mod_helm_chassis.cpp b/src/module/helm_chassis/mod_helm_chassis.cpp
index 45c149e0..2b823c50 100644
--- a/src/module/helm_chassis/mod_helm_chassis.cpp
+++ b/src/module/helm_chassis/mod_helm_chassis.cpp
@@ -55,6 +55,8 @@ static const float kCAP_PERCENTAGE_WORK = (float)CAP_PERCENT_WORK / 100.0f;
 
 #define MOTOR_MAX_SPEED_COFFICIENT 1.2f /* 电机的最大转速 */
 
+// #define MOTOR_ROTATION_MAX_SPEED 9000; /* 电机的最大转速rpm */
+
 using namespace Module;
 
 template <typename Motor, typename MotorParam>
@@ -67,18 +69,18 @@ HelmChassis<Motor, MotorParam>::HelmChassis(Param& param, float control_freq)
   for (int i = 0; i < 4; i++) {
     this->pos_actr_.at(i) =
         new Component::PosActuator(param.pos_param.at(i), control_freq);
-    this->pos_motor_.at(i) =
-        new Motor(param.pos_motor_param.at(i),
-                  (std::string("Chassis_pos_") + std::to_string(i)).c_str());
+    this->pos_motor_.at(i) = new Motor(
+        param.pos_motor_param.at(i),
+        (std::string(string_vector1_[i]) + std::to_string(0)).c_str());
   }
 
   for (uint8_t i = 0; i < 4; i++) {
     this->speed_actr_.at(i) =
         new Component::SpeedActuator(param.speed_param.at(i), control_freq);
 
-    this->speed_motor_.at(i) =
-        new Motor(param.speed_motor_param.at(i),
-                  (std::string("Chassis_speed_") + std::to_string(i)).c_str());
+    this->speed_motor_.at(i) = new Motor(
+        param.speed_motor_param.at(i),
+        (std::string(string_vector2_[i]) + std::to_string(0)).c_str());
   }
   ctrl_lock_.Post();
 
@@ -117,6 +119,10 @@ HelmChassis<Motor, MotorParam>::HelmChassis(Param& param, float control_freq)
     auto yaw_sub = Message::Subscriber<float>("chassis_yaw");
     auto raw_ref_sub = Message::Subscriber<Device::Referee::Data>("referee");
     auto cap_sub = Message::Subscriber<Device::Cap::Info>("cap_info");
+    auto pit_sub = Message::Subscriber<float>("chassis_pitch");
+    auto alpha_sub = Message::Subscriber<double>("chassis_alpha");
+    auto eulr_yaw1_sub = Message::Subscriber<float>("chassis_eulr_yaw1");
+    auto tan_pit_sub = Message::Subscriber<double>("chassis_tan_pit");
 
     uint32_t last_online_time = bsp_time_get_ms();
 
@@ -126,6 +132,10 @@ HelmChassis<Motor, MotorParam>::HelmChassis(Param& param, float control_freq)
       yaw_sub.DumpData(chassis->yaw_);
       raw_ref_sub.DumpData(chassis->raw_ref_);
       cap_sub.DumpData(chassis->cap_);
+      pit_sub.DumpData(chassis->pit_);
+      alpha_sub.DumpData(chassis->alpha_);
+      eulr_yaw1_sub.DumpData(chassis->eulr_yaw1_);
+      tan_pit_sub.DumpData(chassis->tan_pit_);
 
       chassis->ctrl_lock_.Wait(UINT32_MAX);
       /* 更新反馈值 */
@@ -152,7 +162,7 @@ void HelmChassis<Motor, MotorParam>::PraseRef() {
   this->ref_.chassis_power_limit =
       this->raw_ref_.robot_status.chassis_power_limit;
   this->ref_.chassis_pwr_buff = this->raw_ref_.power_heat.chassis_pwr_buff;
-  this->ref_.chassis_watt = this->raw_ref_.power_heat.chassis_watt;
+  // this->ref_.chassis_watt = this->raw_ref_.power_heat.chassis_watt;
   this->ref_.status = this->raw_ref_.status;
 }
 template <typename Motor, typename MotorParam>
@@ -163,7 +173,17 @@ void HelmChassis<Motor, MotorParam>::UpdateFeedback() {
     this->pos_motor_feedback_[i] = this->pos_motor_[i]->GetSpeed();
     this->speed_motor_[i]->Update();
     this->speed_motor_feedback_[i] = this->speed_motor_[i]->GetSpeed();
+  };
+  for (int i = 0; i < 4; ++i) {
+    this->pos_motor_value_[i] = this->pos_motor_[i]->GetAngle().Value();
+    this->speed_motor_value_[i] =
+        M_2PI - this->speed_motor_[i]->GetAngle().Value();
+    this->pos_err_value_[i] = this->pos_actr_[i]->GetLastError();
   }
+  this->wz_test_ = speed_motor_feedback_[0] / 594.04f;
+  this->random_ = Getrandom();
+  this->test_angle01_ = GetRelateAngle();
+  this->test_angle02_ = std::floor(alpha_ / M_PI_4);
 }
 
 template <typename Motor, typename MotorParam>
@@ -176,16 +196,17 @@ bool HelmChassis<Motor, MotorParam>::LimitChassisOutPower(float power_limit,
   }
 
   float sum_motor_power = 0.0f;
-  float motor_3508_power[4];
+  float motor_power[4];
   for (size_t i = 0; i < len; i++) {
-    motor_3508_power[i] =
+    motor_power[i] =
         this->param_.toque_coefficient_3508 * fabsf(motor_out[i]) *
             fabsf(speed_rpm[i]) +
         this->param_.speed_2_coefficient_3508 * speed_rpm[i] * speed_rpm[i] +
         this->param_.out_2_coefficient_3508 * motor_out[i] * motor_out[i];
-    sum_motor_power += motor_3508_power[i];
+    sum_motor_power += motor_power[i];
   }
   sum_motor_power += this->param_.constant_3508;
+  power_ = sum_motor_power;
   if (sum_motor_power > power_limit) {
     for (size_t i = 0; i < len; i++) {
       motor_out[i] *= power_limit / sum_motor_power;
@@ -193,24 +214,104 @@ bool HelmChassis<Motor, MotorParam>::LimitChassisOutPower(float power_limit,
   }
   return true;
 }
+
 template <typename Motor, typename MotorParam>
 uint16_t HelmChassis<Motor, MotorParam>::MAXSPEEDGET(float power_limit) {
   uint16_t speed = 0;
   if (power_limit <= 50.0f) {
     speed = 5000;
   } else if (power_limit <= 60.0f) {
-    speed = 5500;
-  } else if (power_limit <= 70.0f) {
     speed = 6000;
-  } else if (power_limit <= 80.0f) {
+  } else if (power_limit <= 70.0f) {
     speed = 6500;
-  } else if (power_limit <= 100.0f) {
+  } else if (power_limit <= 80.0f) {
     speed = 7000;
-  } else {
+  } else if (power_limit <= 100.0f) {
     speed = 7500;
+  } else {
+    speed = 7000;
   }
   return speed;
 }
+template <typename Motor, typename MotorParam>
+int HelmChassis<Motor, MotorParam>::Getrandom() {
+  static int last_random_value = 1;
+  if (this->mode_ != ROTOR) {
+    this->now_ = bsp_time_get();
+
+    this->dt_ = TIME_DIFF(this->last_wakeup_, this->now_);
+
+    srand(static_cast<unsigned int>(this->now_));
+    int random_value = rand() % 2;
+    if (random_value == 0) {
+      random_value = -1;
+    } else {
+      random_value = 1;
+    };
+
+    last_random_value = random_value;
+  }
+  return last_random_value;
+}
+
+template <typename Motor, typename MotorParam>
+double HelmChassis<Motor, MotorParam>::GetTorqueLength(float angle_a,
+                                                       double angle_b,
+                                                       int selection) {
+  double cofficent = 0, cofficent_1 = 0, cofficent_2 = 0, cofficent_3 = 0,
+         cofficent_4 = 0;
+
+  cofficent_1 = sqrt(0.0557 + 0.03 * tan(angle_a) * tan(angle_a) -
+                     0.081 * tan(angle_a) * cos(3 * M_PI_4 + angle_b));
+  cofficent_2 = sqrt(0.0557 + 0.03 * tan(angle_a) * tan(angle_a) -
+                     0.081 * tan(angle_a) * cos(3 * M_PI_4 - angle_b));
+  cofficent_3 = sqrt(0.0557 + 0.03 * tan(angle_a) * tan(angle_a) -
+                     0.081 * tan(angle_a) * cos(M_PI_4 + angle_b));
+  cofficent_4 = sqrt(0.0557 + 0.03 * tan(angle_a) * tan(angle_a) -
+                     0.081 * tan(angle_a) * cos(M_PI_4 - angle_b));
+  switch (selection) {
+    case 0:
+      cofficent =
+          (1 / ((cofficent_1 / cofficent_2) + (cofficent_1 / cofficent_3) +
+                (cofficent_1 / cofficent_4) + 1)) *
+          2.8 * sin(pit_);
+      break;
+    case 1:
+      cofficent =
+          (1 / ((cofficent_2 / cofficent_1) + (cofficent_2 / cofficent_3) +
+                (cofficent_2 / cofficent_4) + 1)) *
+          2.8 * sin(pit_);
+      break;
+    case 2:
+      cofficent =
+          (1 / ((cofficent_3 / cofficent_1) + (cofficent_3 / cofficent_2) +
+                (cofficent_3 / cofficent_4) + 1)) *
+          2.8 * sin(pit_);
+      break;
+    case 3:
+      cofficent =
+          (1 / ((cofficent_4 / cofficent_1) + (cofficent_4 / cofficent_2) +
+                (cofficent_4 / cofficent_3) + 1)) *
+          2.8 * sin(pit_);
+      break;
+    default:
+      break;
+  }
+  return cofficent;
+}
+
+template <typename Motor, typename MotorParam>
+float HelmChassis<Motor, MotorParam>::GetRelateAngle() {
+  static float last_relate_angle = 0;
+  if (pit_ < 0.1) {
+    float relate_angle = 0;
+    relate_angle = this->eulr_yaw1_;
+
+    last_relate_angle = relate_angle;
+  }
+  return last_relate_angle;
+}
+
 template <typename Motor, typename MotorParam>
 void HelmChassis<Motor, MotorParam>::Control() {
   this->now_ = bsp_time_get();
@@ -218,9 +319,8 @@ void HelmChassis<Motor, MotorParam>::Control() {
   this->dt_ = TIME_DIFF(this->last_wakeup_, this->now_);
 
   this->last_wakeup_ = this->now_;
-  // max_motor_rotational_speed_ = this->MAXSPEEDGET(ref_.chassis_power_limit);
-  max_motor_rotational_speed_ =
-      this->MAXSPEEDGET(this->ref_.chassis_power_limit);
+  max_motor_rotational_speed_ = this->MAXSPEEDGET(ref_.chassis_power_limit);
+
   /* 计算vx、vy */
   switch (this->mode_) {
     case HelmChassis::BREAK: /* 刹车/放松模式电机停止 */
@@ -231,6 +331,7 @@ void HelmChassis<Motor, MotorParam>::Control() {
       /* 独立模式控制向量与运动向量相等 */
     case HelmChassis::INDENPENDENT:
     case HelmChassis::CHASSIS_6020_FOLLOW_GIMBAL: {
+      // float tmp = 0;
       float tmp = sqrtf(cmd_.x * cmd_.x + cmd_.y * cmd_.y) * 1.41421f / 2.0f;
 
       clampf(&tmp, -1.0f, 1.0f);
@@ -244,8 +345,7 @@ void HelmChassis<Motor, MotorParam>::Control() {
       }
       break;
     }
-    case HelmChassis::CHASSIS_FOLLOW_GIMBAL:
-    case HelmChassis::ROTOR: {
+    case HelmChassis::CHASSIS_FOLLOW_GIMBAL: {
       float beta = this->yaw_;
       float cos_beta = cosf(beta);
       float sin_beta = sinf(beta);
@@ -253,6 +353,21 @@ void HelmChassis<Motor, MotorParam>::Control() {
       this->move_vec_.vy = sin_beta * this->cmd_.x + cos_beta * this->cmd_.y;
       break;
     }
+    case HelmChassis::ROTOR: {
+      if (random_ == 1) {
+        deviation_angle_value_ =
+            this->yaw_ + (0.05 * (speed_motor_feedback_[0] / 594.04f));
+      } else {
+        deviation_angle_value_ =
+            this->yaw_ - (0.054 * (speed_motor_feedback_[0] / 594.04f));
+      }
+      float beta = deviation_angle_value_;
+      float cos_beta = cosf(beta);
+      float sin_beta = sinf(beta);
+      this->move_vec_.vx = cos_beta * this->cmd_.x - sin_beta * this->cmd_.y;
+      this->move_vec_.vy = sin_beta * this->cmd_.x + cos_beta * this->cmd_.y;
+      break;
+    }
   }
 
   /* 计算wz */
@@ -278,8 +393,8 @@ void HelmChassis<Motor, MotorParam>::Control() {
       break;
     case HelmChassis::ROTOR:
       /* 陀螺模式底盘以一定速度旋转 */
-      this->move_vec_.wz =
-          this->wz_dir_mult_ * CalcWz(ROTOR_WZ_MIN, ROTOR_WZ_MAX);
+      this->move_vec_.wz = Getrandom();
+      // this->wz_dir_mult_ * CalcWz(ROTOR_WZ_MIN, ROTOR_WZ_MAX);
       break;
     default:
       XB_ASSERT(false);
@@ -310,19 +425,28 @@ void HelmChassis<Motor, MotorParam>::Control() {
       }
       break;
     case HelmChassis::CHASSIS_FOLLOW_GIMBAL:
-    case HelmChassis::ROTOR: {
+    case HelmChassis::ROTOR:
       float x = 0, y = 0, wheel_pos = 0;
       for (int i = 0; i < 4; i++) {
         wheel_pos = -i * M_PI / 2.0f + M_PI / 4.0f * 3.0f;
         x = sinf(wheel_pos) * move_vec_.wz + move_vec_.vx;
         y = cosf(wheel_pos) * move_vec_.wz + move_vec_.vy;
         setpoint_.wheel_pos[i] = -(atan2(y, x) - M_PI / 2.0f);
-        setpoint_.motor_rotational_speed[i] = max_motor_rotational_speed_ *
-                                              sqrtf(x * x + y * y) * 1.41421f /
-                                              2.0f;
+
+        if ((move_vec_.vx < 0.05 && move_vec_.vx > -0.05) ||
+            (move_vec_.vy < 0.05 && move_vec_.vy > -0.05)) {
+          setpoint_.motor_rotational_speed[i] =
+              max_motor_rotational_speed_ * sqrt(x * x + y * y);
+        } else {
+          setpoint_.motor_rotational_speed[i] = max_motor_rotational_speed_ *
+                                                sqrt(x * x + y * y) *
+                                                (1.41421f / 2.0f);
+        }
+        // float tmp = setpoint_.motor_rotational_speed[i];
+        // clampf(&tmp, -max_motor_rotational_speed_,
+        // max_motor_rotational_speed_);
       }
       break;
-    }
   }
 
   /* 寻找电机最近角度 */
@@ -336,19 +460,100 @@ void HelmChassis<Motor, MotorParam>::Control() {
     }
   }
 
+  // //底盘前馈2.0
+  // if (pit_ > 0.01) {
+  //   forward_coefficient_[0] =
+  //       -0.35865 * sin(pit_) * tan(pit_) + 0.913125 * sin(pit_);
+  //   forward_coefficient_[1] =
+  //       -0.35865 * sin(pit_) * tan(pit_) + 0.913125 * sin(pit_);
+  //   forward_coefficient_[2] =
+  //       0.35865 * sin(pit_) * tan(pit_) + 0.95542 * sin(pit_);
+  //   forward_coefficient_[3] =
+  //       0.35865 * sin(pit_) * tan(pit_) + 0.95542 * sin(pit_);
+  // } else {
+  //   for (float& i : forward_coefficient_) {
+  //     i = 0;
+  //   }
+  // }
+
+  /* 底盘前馈3.0 */
+
+  if (pit_ > 0.04 && mode_ != ROTOR && abs(cmd_.y) > 0.01) {
+    constexpr int TERM[8][4] = {{1, 0, 2, 3}, {2, 0, 1, 3}, {3, 1, 0, 2},
+                                {3, 2, 0, 1}, {2, 3, 1, 0}, {1, 3, 2, 0},
+                                {0, 2, 3, 1}, {0, 1, 3, 2}};
+    int state = static_cast<int>(std::floor(alpha_ / M_PI_4));
+    switch (state) {
+      case 0:  // 1023
+        for (int i = 0; i < 4; i++) {
+          forward_coefficient_[i] = GetTorqueLength(pit_, alpha_, TERM[0][i]);
+        }
+        break;
+      case 1:  // 2013
+        for (int i = 0; i < 4; i++) {
+          forward_coefficient_[i] = GetTorqueLength(pit_, alpha_, TERM[1][i]);
+        }
+        break;
+      case 2:  // 3102
+        for (int i = 0; i < 4; i++) {
+          forward_coefficient_[i] = GetTorqueLength(pit_, alpha_, TERM[2][i]);
+        }
+        break;
+      case 3:  // 3201
+        for (int i = 0; i < 4; i++) {
+          forward_coefficient_[i] = GetTorqueLength(pit_, alpha_, TERM[3][i]);
+        }
+        break;
+      case 4:  // 2310
+        for (int i = 0; i < 4; i++) {
+          forward_coefficient_[i] = GetTorqueLength(pit_, alpha_, TERM[4][i]);
+        }
+        break;
+      case 5:  // 1320
+        for (int i = 0; i < 4; i++) {
+          forward_coefficient_[i] = GetTorqueLength(pit_, alpha_, TERM[5][i]);
+        }
+        break;
+      case 6:  // 0231
+        for (int i = 0; i < 4; i++) {
+          forward_coefficient_[i] = GetTorqueLength(pit_, alpha_, TERM[6][i]);
+        }
+        break;
+      case 7:  // 0132
+        for (int i = 0; i < 4; i++) {
+          forward_coefficient_[i] = GetTorqueLength(pit_, alpha_, TERM[7][i]);
+        }
+        break;
+      default:
+        break;
+    }
+  } else {
+    for (float& i : forward_coefficient_) {
+      i = 0;
+    }
+  }
+
+  if ((tan_pit_ < 0 && cmd_.y > 0) || (tan_pit_ > 0 && cmd_.y < 0)) {
+    for (float& i : forward_coefficient_) {
+      i = -i;
+    }
+  }
+
   /* 输出计算 */
   for (int i = 0; i < 4; i++) {
     if (motor_reverse_[i]) {
       out_.speed_motor_out[i] =
           speed_actr_[i]->Calculate(-setpoint_.motor_rotational_speed[i],
-                                    speed_motor_[i]->GetSpeed(), dt_);
+                                    speed_motor_[i]->GetSpeed(), dt_) -
+          forward_coefficient_[i];
       out_.motor6020_out[i] = pos_actr_[i]->Calculate(
           setpoint_.wheel_pos[i] + M_PI + this->param_.mech_zero[i],
           pos_motor_[i]->GetSpeed(), pos_motor_[i]->GetAngle(), dt_);
     } else {
       out_.speed_motor_out[i] =
           speed_actr_[i]->Calculate(setpoint_.motor_rotational_speed[i],
-                                    speed_motor_[i]->GetSpeed(), dt_);
+                                    speed_motor_[i]->GetSpeed(), dt_) +
+          forward_coefficient_[i];
       out_.motor6020_out[i] = pos_actr_[i]->Calculate(
           setpoint_.wheel_pos[i] + this->param_.mech_zero[i],
           pos_motor_[i]->GetSpeed(), pos_motor_[i]->GetAngle(), dt_);
@@ -368,20 +573,24 @@ void HelmChassis<Motor, MotorParam>::Control() {
 
   clampf(&percentage, 0.0f, 1.0f);
   /* 控制 */
-  float max_power_limit = ref_.chassis_power_limit + ref_.chassis_power_limit *
-                                                         0.2 *
-                                                         this->cap_.percentage_;
+  // float max_power_limit = ref_.chassis_power_limit + ref_.chassis_power_limit
+  // *
+  //                                                        1.8 *
+  //                                                        this->cap_.percentage_;
+
+  // float max_power_limit = ref_.chassis_power_limit;
+
   // sum_6020_out_ =
   //     Calculate6020Power(out_.motor6020_out, motor_feedback_.pos_speed, 4);
   for (int i = 0; i < 4; i++) {
     if (cap_.online_) {
-      LimitChassisOutPower(max_power_limit, out_.speed_motor_out,
-                           speed_motor_feedback_, 4);
+      LimitChassisOutPower(180.0f, out_.speed_motor_out, speed_motor_feedback_,
+                           4);
       this->speed_motor_[i]->Control(out_.speed_motor_out[i]);
       this->pos_motor_[i]->Control(out_.motor6020_out[i]);
     } else {
-      LimitChassisOutPower(max_power_limit, out_.speed_motor_out,
-                           speed_motor_feedback_, 4);
+      LimitChassisOutPower(500.f, out_.speed_motor_out, speed_motor_feedback_,
+                           4);
       this->speed_motor_[i]->Control(out_.speed_motor_out[i]);
       this->pos_motor_[i]->Control(out_.motor6020_out[i]);
     }
diff --git a/src/module/helm_chassis/mod_helm_chassis.hpp b/src/module/helm_chassis/mod_helm_chassis.hpp
index b120d573..cf494311 100644
--- a/src/module/helm_chassis/mod_helm_chassis.hpp
+++ b/src/module/helm_chassis/mod_helm_chassis.hpp
@@ -16,7 +16,9 @@
 #include <array>
 #include <cmath>
 #include <comp_type.hpp>
+#include <cstdlib>
 #include <module.hpp>
+#include <vector>
 
 #include "comp_actuator.hpp"
 #include "comp_cmd.hpp"
@@ -27,7 +29,7 @@
 #include "dev_motor.hpp"
 #include "dev_referee.hpp"
 #include "dev_rm_motor.hpp"
-#define PI (M_2PI)
+#define PI_ (M_2PI)
 
 namespace Module {
 template <typename Motor, typename MotorParam>
@@ -99,6 +101,12 @@ class HelmChassis {
 
   float CalcWz(const float LO, const float HI);
 
+  int Getrandom();  //随机数
+
+  double GetTorqueLength(float angle_a, double angle_b, int selection);
+
+  float GetRelateAngle();  //相对偏航角
+
  private:
   Param param_;
 
@@ -108,17 +116,38 @@ class HelmChassis {
 
   uint64_t now_ = 0;
   float yaw_;
-
+  float pit_;
+  double alpha_;
+  float pos_motor_value_[4];
+  float speed_motor_value_[4];
+  float pos_err_value_[4];
+  float wz_test_;
   float pos_motor_sum_out_;
+  float eulr_yaw1_;
+  int random_;
+  float deviation_angle_value_;
+  float test_angle01_;
+  float test_angle02_;
+  double tan_pit_;
+  std::vector<std::string> string_vector1_{
+      "Chassis_pos_RightFront", "Chassis_pos_LeftFront", "Chassis_pos_LeftBack",
+      "Chassis_pos_RightBack"};
+
+  std::vector<std::string> string_vector2_{
+      "Chassis_speed_RightFront", "Chassis_speed_LeftFront",
+      "Chassis_speed_LeftBack", "Chassis_speed_RightBack"};
 
   float max_motor_rotational_speed_;
+
   Device::Cap::Info cap_;
 
   Device::Referee::Data raw_ref_;
 
   float speed_motor_feedback_[4];
   float pos_motor_feedback_[4];
-
+  float forward_coefficient_[4];
+  float l_data_[4];
+  float power_;
   RefForChassis ref_;
 
   Mode mode_ = RELAX;
@@ -150,6 +179,8 @@ class HelmChassis {
 
   float direct_offset_ = 0.0f;
 
+  float state_num_;
+
   Component::PID follow_pid_;
 
   System::Thread thread_;
@@ -167,6 +198,10 @@ class HelmChassis {
   std::array<float, 4> speed_motor_out_;
 
   std::array<float, 4> pos_motor_out_;
+
+  Component::Type::Eulr eulr_;
+  Component::Type::Quaternion quat_;
+  Component::Type::Vector3 gyro_;
 };
 
 typedef HelmChassis<Device::RMMotor, Device::RMMotor::Param> RMHelmChassis;
diff --git a/src/module/launcher/mod_launcher.cpp b/src/module/launcher/mod_launcher.cpp
index 7df68f5d..c38544a4 100644
--- a/src/module/launcher/mod_launcher.cpp
+++ b/src/module/launcher/mod_launcher.cpp
@@ -3,28 +3,28 @@
 #include "bsp_pwm.h"
 #include "bsp_time.h"
 
-#define LAUNCHER_TRIG_SPEED_MAX (8191)
+#define LAUNCHER_TRIG_SPEED_MAX (16000)
 
 using namespace Module;
 
-Launcher::Launcher(Param& param, float control_freq)
+template <typename Motor, typename Motorparam, int Fric_num, int Trig_num>
+Launcher<Motor, Motorparam, Fric_num, Trig_num>::Launcher(Param& param,
+                                                          float control_freq)
     : param_(param), ctrl_lock_(true) {
-  for (size_t i = 0; i < LAUNCHER_ACTR_TRIG_NUM; i++) {
+  for (size_t i = 0; i < Trig_num; i++) {
     this->trig_actuator_.at(i) =
         new Component::PosActuator(param.trig_actr.at(i), control_freq);
 
-    this->trig_motor_.at(i) =
-        new Device::RMMotor(this->param_.trig_motor.at(i),
-                            ("Launcher_Trig" + std::to_string(i)).c_str());
+    this->trig_motor_.at(i) = new Motor(
+        param.trig_param.at(i), ("Launcher_Trig" + std::to_string(i)).c_str());
   }
 
-  for (size_t i = 0; i < LAUNCHER_ACTR_FRIC_NUM; i++) {
+  for (size_t i = 0; i < Fric_num; i++) {
     this->fric_actuator_.at(i) =
         new Component::SpeedActuator(param.fric_actr.at(i), control_freq);
 
-    this->fric_motor_.at(i) =
-        new Device::RMMotor(this->param_.fric_motor.at(i),
-                            ("Launcher_Fric" + std::to_string(i)).c_str());
+    this->fric_motor_.at(i) = new Motor(
+        param.fric_param.at(i), ("Launcher_Fric" + std::to_string(i)).c_str());
   }
 
   auto event_callback = [](LauncherEvent event, Launcher* launcher) {
@@ -76,8 +76,8 @@ Launcher::Launcher(Param& param, float control_freq)
   Component::CMD::RegisterEvent<Launcher*, LauncherEvent>(
       event_callback, this, this->param_.EVENT_MAP);
 
-  bsp_pwm_start(BSP_PWM_LAUNCHER_SERVO);
-  bsp_pwm_set_comp(BSP_PWM_LAUNCHER_SERVO, this->param_.cover_close_duty);
+  // bsp_pwm_start(BSP_PWM_LAUNCHER_SERVO);
+  // bsp_pwm_set_comp(BSP_PWM_LAUNCHER_SERVO, this->param_.cover_close_duty);
 
   auto launcher_thread = [](Launcher* launcher) {
     auto ref_sub = Message::Subscriber<Device::Referee::Data>("referee");
@@ -109,15 +109,16 @@ Launcher::Launcher(Param& param, float control_freq)
   System::Timer::Create(this->DrawUIDynamic, this, 100);
 }
 
-void Launcher::UpdateFeedback() {
+template <typename Motor, typename Motorparam, int Fric_num, int Trig_num>
+void Launcher<Motor, Motorparam, Fric_num, Trig_num>::UpdateFeedback() {
   const float LAST_TRIG_MOTOR_ANGLE = this->trig_motor_[0]->GetAngle();
 
-  for (size_t i = 0; i < LAUNCHER_ACTR_FRIC_NUM; i++) {
+  for (size_t i = 0; i < Fric_num; i++) {
     this->fric_motor_[i]->Update();
     speed[i] = fric_motor_[i]->GetSpeed();
   }
 
-  for (size_t i = 0; i < LAUNCHER_ACTR_TRIG_NUM; i++) {
+  for (size_t i = 0; i < Trig_num; i++) {
     this->trig_motor_[i]->Update();
   }
 
@@ -126,7 +127,8 @@ void Launcher::UpdateFeedback() {
   this->trig_angle_ += DELTA_MOTOR_ANGLE / this->param_.trig_gear_ratio;
 }
 
-void Launcher::Control() {
+template <typename Motor, typename Motorparam, int Fric_num, int Trig_num>
+void Launcher<Motor, Motorparam, Fric_num, Trig_num>::Control() {
   this->now_ = bsp_time_get();
   this->dt_ = TIME_DIFF(this->last_wakeup_, this->now_);
 
@@ -236,19 +238,21 @@ void Launcher::Control() {
   /* 计算摩擦轮和拨弹盘并输出 */
   switch (this->fire_ctrl_.fire_mode_) {
     case RELAX:
-      for (size_t i = 0; i < LAUNCHER_ACTR_TRIG_NUM; i++) {
+      for (size_t i = 0; i < Trig_num; i++) {
         this->trig_motor_[i]->Relax();
       }
-      for (size_t i = 0; i < LAUNCHER_ACTR_FRIC_NUM; i++) {
+      for (size_t i = 0; i < Fric_num; i++) {
         this->fric_motor_[i]->Relax();
       }
-      bsp_pwm_stop(BSP_PWM_LAUNCHER_SERVO);
+      // sp_pwm_stop(BSP_PWM_LAUNCHER_SERVO);
       break;
 
     case SAFE:
     case LOADED:
-      for (int i = 0; i < LAUNCHER_ACTR_TRIG_NUM; i++) {
+      for (size_t i = 0; i < Trig_num; i++) {
         /* 控制拨弹电机 */
+        float trig_speed = this->trig_motor_[i]->GetSpeed();
+        clampf(&trig_speed, 0.f, LAUNCHER_TRIG_SPEED_MAX);
         float trig_out = this->trig_actuator_[i]->Calculate(
             this->setpoint_.trig_angle_,
             this->trig_motor_[i]->GetSpeed() / LAUNCHER_TRIG_SPEED_MAX,
@@ -257,7 +261,7 @@ void Launcher::Control() {
         this->trig_motor_[i]->Control(trig_out);
       }
 
-      for (size_t i = 0; i < LAUNCHER_ACTR_FRIC_NUM; i++) {
+      for (size_t i = 0; i < Fric_num; i++) {
         /* 控制摩擦轮 */
         float fric_out = this->fric_actuator_[i]->Calculate(
             this->setpoint_.fric_rpm_[i], this->fric_motor_[i]->GetSpeed(),
@@ -267,19 +271,23 @@ void Launcher::Control() {
       }
 
       /* 根据弹仓盖开关状态更新弹舱盖打开时舵机PWM占空比 */
-      if (this->cover_mode_ == OPEN) {
-        bsp_pwm_start(BSP_PWM_LAUNCHER_SERVO);
-        bsp_pwm_set_comp(BSP_PWM_LAUNCHER_SERVO, this->param_.cover_open_duty);
-      } else {
-        bsp_pwm_start(BSP_PWM_LAUNCHER_SERVO);
-        bsp_pwm_set_comp(BSP_PWM_LAUNCHER_SERVO, this->param_.cover_close_duty);
-      }
+      // if (this->cover_mode_ == OPEN) {
+      //   bsp_pwm_start(BSP_PWM_LAUNCHER_SERVO);
+      //   bsp_pwm_set_comp(BSP_PWM_LAUNCHER_SERVO,
+      //   this->param_.cover_open_duty);
+      // } else {
+      //   bsp_pwm_start(BSP_PWM_LAUNCHER_SERVO);
+      //   bsp_pwm_set_comp(BSP_PWM_LAUNCHER_SERVO,
+      //   this->param_.cover_close_duty);
+      // }
       break;
   }
 }
 /* 拨弹盘模式 */
 /* SINGLE,BURST,CONTINUED,  */
-void Launcher::SetTrigMode(TrigMode mode) {
+template <typename Motor, typename Motorparam, int Fric_num, int Trig_num>
+void Launcher<Motor, Motorparam, Fric_num, Trig_num>::SetTrigMode(
+    TrigMode mode) {
   if (mode == this->fire_ctrl_.trig_mode_) {
     return;
   }
@@ -288,14 +296,16 @@ void Launcher::SetTrigMode(TrigMode mode) {
 }
 /* 设置摩擦轮模式 */
 /* RELEX SAFE LOADED三种模式可以选择 */
-void Launcher::SetFireMode(FireMode mode) {
+template <typename Motor, typename Motorparam, int Fric_num, int Trig_num>
+void Launcher<Motor, Motorparam, Fric_num, Trig_num>::SetFireMode(
+    FireMode mode) {
   if (mode == this->fire_ctrl_.fire_mode_) { /* 未更改,return */
     return;
   }
 
   fire_ctrl_.fire = false;
 
-  for (size_t i = 0; i < LAUNCHER_ACTR_FRIC_NUM; i++) {
+  for (size_t i = 0; i < Fric_num; i++) {
     this->fric_actuator_[i]->Reset();
   } /* reset 所有电机执行器PID等参数 */
 
@@ -306,7 +316,8 @@ void Launcher::SetFireMode(FireMode mode) {
   this->fire_ctrl_.fire_mode_ = mode;
 }
 
-void Launcher::HeatLimit() {
+template <typename Motor, typename Motorparam, int Fric_num, int Trig_num>
+void Launcher<Motor, Motorparam, Fric_num, Trig_num>::HeatLimit() {
   if (this->ref_.status == Device::Referee::RUNNING) {
     /* 根据机器人型号获得对应数据 */
     if (this->param_.model == LAUNCHER_MODEL_42MM) {
@@ -340,7 +351,8 @@ void Launcher::HeatLimit() {
   }
 }
 
-void Launcher::PraseRef() {
+template <typename Motor, typename Motorparam, int Fric_num, int Trig_num>
+void Launcher<Motor, Motorparam, Fric_num, Trig_num>::PraseRef() {
   memcpy(&(this->ref_.power_heat), &(this->raw_ref_.power_heat),
          sizeof(this->ref_.power_heat));
   memcpy(&(this->ref_.robot_status), &(this->raw_ref_.robot_status),
@@ -350,29 +362,51 @@ void Launcher::PraseRef() {
   this->ref_.status = this->raw_ref_.status;
 }
 
-float Launcher::LimitLauncherFreq() { /* 热量限制计算 */
+// 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; /* 每秒可发弹量 */
+//   if (this->param_.model == LAUNCHER_MODEL_42MM) {
+//     return stable_freq;
+//   } else {
+//     if (heat_percent > 0.9f) {
+//       return 0.5f;
+//     } else if (heat_percent > 0.8f) {
+//       return 1.0f;
+//     } else if (heat_percent > 0.6f) {
+//       return 2.0f * stable_freq;
+//     } else if (heat_percent > 0.2f) {
+//       return 3.0f * stable_freq;
+//     } else if (heat_percent > 0.1f) {
+//       return 4.0f * stable_freq;
+//     } else {
+//       return 5.0f;
+//     }
+//   }
+// }
+
+template <typename Motor, typename Motorparam, int Fric_num, int Trig_num>
+float Launcher<Motor, Motorparam, Fric_num,
+               Trig_num>::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; /* 每秒可发弹量 */
-  if (this->param_.model == LAUNCHER_MODEL_42MM) {
-    return stable_freq;
+  if (heat_percent > 0.9f) {
+    return 0.5f;
+  } else if (heat_percent > 0.85f) {
+    return stable_freq * 0.6f;
+  } else if (heat_percent > 0.8f) {
+    return 0.6f * 1000 / this->param_.min_launch_delay;
+  } else if (heat_percent > 0.6f) {
+    return 0.8f * 1000 / this->param_.min_launch_delay;
   } else {
-    if (heat_percent > 0.9f) {
-      return 0.5f;
-    } else if (heat_percent > 0.8f) {
-      return 1.0f;
-    } else if (heat_percent > 0.6f) {
-      return 2.0f * stable_freq;
-    } else if (heat_percent > 0.2f) {
-      return 3.0f * stable_freq;
-    } else if (heat_percent > 0.1f) {
-      return 4.0f * stable_freq;
-    } else {
-      return 5.0f;
-    }
+    return 1000 / this->param_.min_launch_delay;
   }
 }
-void Launcher::DrawUIStatic(Launcher* launcher) {
+
+template <typename Motor, typename Motorparam, int Fric_num, int Trig_num>
+void Launcher<Motor, Motorparam, Fric_num, Trig_num>::DrawUIStatic(
+    Launcher* launcher) {
   launcher->string_.Draw("SM", Component::UI::UI_GRAPHIC_OP_ADD,
                          Component::UI::UI_GRAPHIC_LAYER_CONST,
                          Component::UI::UI_GREEN, UI_DEFAULT_WIDTH * 10, 80,
@@ -511,7 +545,9 @@ void Launcher::DrawUIStatic(Launcher* launcher) {
   Device::Referee::AddUI(launcher->arc_);
 }
 
-void Launcher::DrawUIDynamic(Launcher* launcher) {
+template <typename Motor, typename Motorparam, int Fric_num, int Trig_num>
+void Launcher<Motor, Motorparam, Fric_num, Trig_num>::DrawUIDynamic(
+    Launcher* launcher) {
   float box_pos_left = 0.0f, box_pos_right = 0.0f;
 
   /* 更新发射器模式选择框 */
@@ -655,3 +691,5 @@ void Launcher::DrawUIDynamic(Launcher* launcher) {
       40, 40);
   Device::Referee::AddUI(launcher->arc_);
 }
+
+template class Module::Launcher<Device::RMMotor, Device::RMMotor::Param>;
diff --git a/src/module/launcher/mod_launcher.hpp b/src/module/launcher/mod_launcher.hpp
index b569c249..138528ac 100644
--- a/src/module/launcher/mod_launcher.hpp
+++ b/src/module/launcher/mod_launcher.hpp
@@ -10,6 +10,8 @@
 #include "dev_rm_motor.hpp"
 
 namespace Module {
+template <typename Motor, typename Motorparam, int Fric_num = 2,
+          int Trig_num = 1>
 class Launcher {
  public:
   /* 发射器运行模式 */
@@ -44,24 +46,6 @@ class Launcher {
     CLOSE_COVER,
   } LauncherEvent;
 
-  enum {
-    LAUNCHER_ACTR_FRIC1_IDX = 0, /* 1号摩擦轮相关的索引值 */
-    LAUNCHER_ACTR_FRIC2_IDX,     /* 2号摩擦轮相关的索引值 */
-    LAUNCHER_ACTR_FRIC_NUM,      /* 总共的动作器数量 */
-  };
-
-  enum {
-    LAUNCHER_ACTR_TRIG_IDX, /* 拨弹电机相关的索引值 */
-    LAUNCHER_ACTR_TRIG_NUM, /* 总共的动作器数量 */
-  };
-
-  enum {
-    LAUNCHER_CTRL_FRIC1_SPEED_IDX = 0, /* 摩擦轮1控制的速度环控制器的索引值 */
-    LAUNCHER_CTRL_FRIC2_SPEED_IDX, /* 摩擦轮2控制的速度环控制器的索引值 */
-    LAUNCHER_CTRL_TRIG_SPEED_IDX, /* 拨弹电机控制的速度环控制器的索引值 */
-    LAUNCHER_CTRL_NUM,            /* 总共的控制器数量 */
-  };
-
   typedef enum {
     LAUNCHER_MODEL_17MM = 0, /* 17mm发射机构 */
     LAUNCHER_MODEL_42MM,     /* 42mm发射机构 */
@@ -77,11 +61,10 @@ class Launcher {
     float default_bullet_speed; /* 默认弹丸初速度 */
     uint32_t min_launch_delay;  /* 最小发射间隔(1s/最大射频) */
 
-    std::array<Component::PosActuator::Param, LAUNCHER_ACTR_TRIG_NUM> trig_actr;
-    std::array<Component::SpeedActuator::Param, LAUNCHER_ACTR_FRIC_NUM>
-        fric_actr;
-    std::array<Device::RMMotor::Param, LAUNCHER_ACTR_TRIG_NUM> trig_motor;
-    std::array<Device::RMMotor::Param, LAUNCHER_ACTR_FRIC_NUM> fric_motor;
+    std::array<Component::PosActuator::Param, Trig_num> trig_actr;
+    std::array<Component::SpeedActuator::Param, Fric_num> fric_actr;
+    std::array<Motorparam, Trig_num> trig_param;
+    std::array<Motorparam, Fric_num> fric_param;
 
     const std::vector<Component::CMD::EventMapItem> EVENT_MAP;
   } Param;
@@ -165,11 +148,11 @@ class Launcher {
   HeatControl heat_ctrl_;
   FireControl fire_ctrl_;
 
-  std::array<Component::PosActuator *, LAUNCHER_ACTR_TRIG_NUM> trig_actuator_;
-  std::array<Component::SpeedActuator *, LAUNCHER_ACTR_FRIC_NUM> fric_actuator_;
+  std::array<Component::PosActuator *, Trig_num> trig_actuator_;
+  std::array<Component::SpeedActuator *, Fric_num> fric_actuator_;
 
-  std::array<Device::RMMotor *, LAUNCHER_ACTR_TRIG_NUM> trig_motor_;
-  std::array<Device::RMMotor *, LAUNCHER_ACTR_FRIC_NUM> fric_motor_;
+  std::array<Device::RMMotor *, Trig_num> trig_motor_;
+  std::array<Device::RMMotor *, Fric_num> fric_motor_;
 
   RefForLauncher ref_;
 
@@ -185,4 +168,5 @@ class Launcher {
 
   Component::UI::Arc arc_;
 };
+typedef class Launcher<Device::RMMotor, Device::RMMotor::Param> RMLauncher;
 }  // namespace Module
diff --git a/src/module/omni_chassis/mod_omni_chassis.cpp b/src/module/omni_chassis/mod_omni_chassis.cpp
index 6f11c638..1fd94de4 100644
--- a/src/module/omni_chassis/mod_omni_chassis.cpp
+++ b/src/module/omni_chassis/mod_omni_chassis.cpp
@@ -17,12 +17,13 @@
 
 #include "bsp_time.h"
 
-#define ROTOR_WZ_MIN 0.8f /* 小陀螺旋转位移下界 */
-#define ROTOR_WZ_MAX 1.0f /* 小陀螺旋转位移上界 */
+#define ROTOR_WZ_MIN 0.6f /* 小陀螺旋转位移下界 */
+#define ROTOR_WZ_MAX 0.8f /* 小陀螺旋转位移上界 */
 
 #define ROTOR_OMEGA 0.0025f /* 小陀螺转动频率 */
 
 #define MOTOR_MAX_SPEED_COFFICIENT 1.2f /* 电机的最大转速 */
+#define MOTOR_MAX_ROTATIONAL_SPEED 9600 /* 电机最大转速 */
 
 #if POWER_LIMIT_WITH_CAP
 /* 保证电容电量宏定义在正确范围内 */
@@ -44,11 +45,13 @@ static const float kCAP_PERCENTAGE_WORK = (float)CAP_PERCENT_WORK / 100.0f;
 using namespace Module;
 
 template <typename Motor, typename MotorParam>
-OmniChassis<Motor, MotorParam>::OmniChassis(Param& param, float control_freq)
+nOmniChassis<Motor, MotorParam>::nOmniChassis(Param& param, float control_freq)
     : param_(param),
-      mode_(OmniChassis::RELAX),
+      mode_(nOmniChassis::RELAX),
       mixer_(param.type),
       follow_pid_(param.follow_pid_param, control_freq),
+      xaccl_pid_(param.xaccl_pid_param, control_freq),
+      yaccl_pid_(param.yaccl_pid_param, control_freq),
       ctrl_lock_(true) {
   memset(&(this->cmd_), 0, sizeof(this->cmd_));
 
@@ -61,12 +64,9 @@ OmniChassis<Motor, MotorParam>::OmniChassis(Param& param, float control_freq)
                   (std::string("Chassis_") + std::to_string(i)).c_str());
   }
 
-  this->setpoint_.motor_rotational_speed =
-      reinterpret_cast<float*>(System::Memory::Malloc(
-          this->mixer_.len_ * sizeof(*this->setpoint_.motor_rotational_speed)));
   XB_ASSERT(this->setpoint_.motor_rotational_speed);
 
-  auto event_callback = [](ChassisEvent event, OmniChassis* chassis) {
+  auto event_callback = [](ChassisEvent event, nOmniChassis* chassis) {
     chassis->ctrl_lock_.Wait(UINT32_MAX);
 
     switch (event) {
@@ -94,10 +94,10 @@ OmniChassis<Motor, MotorParam>::OmniChassis(Param& param, float control_freq)
     chassis->ctrl_lock_.Post();
   };
 
-  Component::CMD::RegisterEvent<OmniChassis*, ChassisEvent>(
+  Component::CMD::RegisterEvent<nOmniChassis*, ChassisEvent>(
       event_callback, this, this->param_.EVENT_MAP);
 
-  auto chassis_thread = [](OmniChassis* chassis) {
+  auto chassis_thread = [](nOmniChassis* chassis) {
     auto raw_ref_sub = Message::Subscriber<Device::Referee::Data>("referee");
     auto cmd_sub =
         Message::Subscriber<Component::CMD::ChassisCMD>("cmd_chassis");
@@ -137,36 +137,26 @@ OmniChassis<Motor, MotorParam>::OmniChassis(Param& param, float control_freq)
 }
 
 template <typename Motor, typename MotorParam>
-void OmniChassis<Motor, MotorParam>::UpdateFeedback() {
+void nOmniChassis<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();
+    this->motor_speed_[i] = this->motor_[i]->GetSpeed();
   }
 }
 template <typename Motor, typename MotorParam>
-uint16_t OmniChassis<Motor, MotorParam>::MAXSPEEDGET(float power_limit) {
-  uint16_t speed = 0.0f;
-  if (power_limit <= 50.0f) {
-    speed = 5000;
-  } else if (power_limit <= 60.0f) {
-    speed = 5500;
-  } else if (power_limit <= 70.0f) {
-    speed = 5500;
-  } else if (power_limit <= 80.0f) {
-    speed = 6500;
-  } else if (power_limit <= 100.0f) {
-    speed = 7000;
+uint16_t nOmniChassis<Motor, MotorParam>::MAXSPEEDGET(float power_limit) {
+  if (param_.get_speed) {
+    return param_.get_speed(power_limit);
   } else {
-    speed = 7500;
+    return 5500;
   }
-  return speed;
 }
 template <typename Motor, typename MotorParam>
-bool OmniChassis<Motor, MotorParam>::LimitChassisOutput(float power_limit,
-                                                        float* motor_out,
-                                                        float* speed_rpm,
-                                                        uint32_t len) {
+bool nOmniChassis<Motor, MotorParam>::LimitChassisOutput(float power_limit,
+                                                         float* motor_out,
+                                                         float* speed_rpm,
+                                                         uint32_t len) {
   if (power_limit < 0.0f) {
     return 0;
   }
@@ -209,34 +199,22 @@ bool OmniChassis<Motor, MotorParam>::LimitChassisOutput(float power_limit,
   return 0;
 }
 template <typename Motor, typename MotorParam>
-bool OmniChassis<Motor, MotorParam>::LimitChassisOutPower(float power_limit,
-                                                          float* motor_out,
-                                                          float* speed_rpm,
-                                                          uint32_t len) {
+bool nOmniChassis<Motor, MotorParam>::LimitChassisOutPower(float power_limit,
+                                                           float* motor_out,
+                                                           float* speed_rpm,
+                                                           uint32_t len) {
   if (power_limit < 0.0f) {
     return 0;
   }
-  // float sum_motor_out = 0.0f;
-  // for (size_t i = 0; i < len; i++) {
-  //   sum_motor_out += fabsf(motor_out[i]) * fabsf(speed[i]) * 0.06f;
-  // }
-  // sum_motor_out += 9.2326f;
-  // power_1_ = sum_motor_out;
-  // if (sum_motor_out > power_limit) {
-  //   for (size_t i = 0; i < len; i++) {
-  //     motor_out[i] *= power_limit / sum_motor_out;
-  //   }
-  // }
-  // return true;
   float sum_motor_power = 0.0f;
-  float motor_3508_power[len];
+  float motor_power[4];
   for (size_t i = 0; i < len; i++) {
-    motor_3508_power[i] =
+    motor_power[i] =
         this->param_.toque_coefficient_ * fabsf(motor_out[i]) *
             fabsf(speed_rpm[i]) +
         this->param_.speed_2_coefficient_ * speed_rpm[i] * speed_rpm[i] +
         this->param_.out_2_coefficient_ * motor_out[i] * motor_out[i];
-    sum_motor_power += motor_3508_power[i];
+    sum_motor_power += motor_power[i];
   }
   sum_motor_power += this->param_.constant_;
   power_ = sum_motor_power;
@@ -249,7 +227,7 @@ bool OmniChassis<Motor, MotorParam>::LimitChassisOutPower(float power_limit,
   return true;
 }
 template <typename Motor, typename MotorParam>
-void OmniChassis<Motor, MotorParam>::Control() {
+void nOmniChassis<Motor, MotorParam>::Control() {
   this->now_ = bsp_time_get();
 
   this->dt_ = TIME_DIFF(this->last_wakeup_, this->now_);
@@ -260,27 +238,58 @@ void OmniChassis<Motor, MotorParam>::Control() {
   /* ctrl_vec -> move_vec 控制向量和真实的移动向量之间有一个换算关系 */
   /* 计算vx、vy */
   switch (this->mode_) {
-    case OmniChassis::BREAK: /* 刹车模式电机停止 */
+    case nOmniChassis::BREAK: /* 刹车模式电机停止 */
       this->move_vec_.vx = 0.0f;
       this->move_vec_.vy = 0.0f;
       break;
 
-    case OmniChassis::INDENPENDENT: /* 独立模式控制向量与运动向量相等
-                                     */
+    case nOmniChassis::INDENPENDENT: /* 独立模式控制向量与运动向量相等
+                                      */
       this->move_vec_.vx = this->cmd_.x;
       this->move_vec_.vy = this->cmd_.y;
       break;
 
-    case OmniChassis::RELAX:
-    case OmniChassis::FOLLOW_GIMBAL_INTERSECT: /* 按照云台方向换算运动向量
-                                                */
-    case OmniChassis::FOLLOW_GIMBAL_CROSS:
-    case OmniChassis::ROTOR: {
+    case nOmniChassis::RELAX:
+    case nOmniChassis::FOLLOW_GIMBAL_INTERSECT: /* 按照云台方向换算运动向量 */
+    case nOmniChassis::FOLLOW_GIMBAL_CROSS: {
       float beta = this->yaw_;
       float cos_beta = cosf(beta);
       float sin_beta = sinf(beta);
+      /*控制加速度*/
+      this->move_vec_.vx = this->xaccl_pid_.Calculate(
+          (cos_beta * this->cmd_.x - sin_beta * this->cmd_.y),
+          this->move_vec_.vx, dt_);
+      if (!cmd_.x) {
+        this->xaccl_pid_.Reset();
+      }
+      this->move_vec_.vy = this->yaccl_pid_.Calculate(
+          (sin_beta * this->cmd_.x + cos_beta * this->cmd_.y),
+          this->move_vec_.vy, dt_);
+      if (!cmd_.y) {
+        this->yaccl_pid_.Reset();
+      }
+      float scalar_sum = fabs(this->move_vec_.vx) + fabs(this->move_vec_.vy);
+      if (scalar_sum > 1.01f) {
+        this->move_vec_.vx = this->move_vec_.vx / scalar_sum;
+        this->move_vec_.vy = this->move_vec_.vy / scalar_sum;
+      }
+    } break;
+    case nOmniChassis::ROTOR: {
+      float beta = this->yaw_ - M_PI / 8.0f;
+      // if (this->move_vec_.wz == 1) {
+      //   beta = this->yaw_ - M_PI_4;
+      // } else {
+      //   beta = this->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;
       this->move_vec_.vy = sin_beta * this->cmd_.x + cos_beta * this->cmd_.y;
+      float scalar_sum = fabs(this->move_vec_.vx) + fabs(this->move_vec_.vy);
+      if (scalar_sum > 1.01f) {
+        this->move_vec_.vx = this->move_vec_.vx / scalar_sum;
+        this->move_vec_.vy = this->move_vec_.vy / scalar_sum;
+      }
       break;
     }
     default:
@@ -289,25 +298,74 @@ void OmniChassis<Motor, MotorParam>::Control() {
 
   /* 计算wz */
   switch (this->mode_) {
-    case OmniChassis::RELAX:
-    case OmniChassis::BREAK:
-    case OmniChassis::INDENPENDENT: /* 独立模式wz为0 */
+    case nOmniChassis::RELAX:
+    case nOmniChassis::BREAK:
+    case nOmniChassis::INDENPENDENT: /* 独立模式wz为0 */
       this->move_vec_.wz = this->cmd_.z;
       break;
 
-    case OmniChassis::FOLLOW_GIMBAL_INTERSECT:
+    case nOmniChassis::FOLLOW_GIMBAL_INTERSECT:
+      //{
+      //   float direction = 0.0f;
+      //   /*双零点*/
+      //   if (this->yaw_ > M_PI_2) {
+      //     direction = 3.14158f;
+      //   }
+      //   if (this->yaw_ < -M_PI_2) {
+      //     direction = 3.14158f;
+      //   }
+      //   this->move_vec_.wz =
+      //       this->follow_pid_.Calculate(direction, this->yaw_, this->dt_);
+      //   clampf(&this->move_vec_.wz, -1.0f, 1.0f);
+      //   float move_scal_sum = fabs(this->move_vec_.vx) +
+      //                         fabs(this->move_vec_.vy) +
+      //                         fabs(this->move_vec_.wz);
+      //   if (move_scal_sum > 1.01f) {
+      //     this->move_vec_.vx =
+      //         this->move_vec_.vx * (1 - fabs(this->move_vec_.wz));
+      //     this->move_vec_.vy =
+      //         this->move_vec_.vy * (1 - fabs(this->move_vec_.wz));
+      //   }
+      // } break;
       this->move_vec_.wz =
           this->follow_pid_.Calculate(0.0f, this->yaw_, this->dt_);
       break;
-    case OmniChassis::FOLLOW_GIMBAL_CROSS:
+    case nOmniChassis::FOLLOW_GIMBAL_CROSS: {
+      // float direction = 0.0f;
+      // /*双零点*/
+      // if (this->yaw_ > M_PI_2) {
+      //   direction = 3.14158f;
+      // }
+      // if (this->yaw_ < -M_PI_2) {
+      //   direction = 3.14158f;
+      // }
+      // this->move_vec_.wz = this->follow_pid_.Calculate(
+      //     direction, this->yaw_ - M_PI / 4.0f, this->dt_);
+      // clampf(&this->move_vec_.wz, -1.0f, 1.0f);
+      // float move_scal_sum = fabs(this->move_vec_.vx) +
+      //                       fabs(this->move_vec_.vy) +
+      //                       fabs(this->move_vec_.wz);
+      // if (move_scal_sum > 1.01f) {
+      //   this->move_vec_.vx =
+      //       this->move_vec_.vx * (1 - fabs(this->move_vec_.wz));
+      //   this->move_vec_.vy =
+      //       this->move_vec_.vy * (1 - fabs(this->move_vec_.wz));
+      // }
       this->move_vec_.wz = this->follow_pid_.Calculate(
           0.0f, this->yaw_ - M_PI / 4.0f, this->dt_);
-      break;
-
-    case OmniChassis::ROTOR: { /* 小陀螺模式使底盘以一定速度旋转
-                                */
-      this->move_vec_.wz =
-          this->wz_dir_mult_ * CalcWz(ROTOR_WZ_MIN, ROTOR_WZ_MAX);
+    } break;
+
+    case nOmniChassis::ROTOR: /* 小陀螺模式使底盘以一定速度旋转 */
+    {                         /* TODO 改成实际底盘速度 */
+      this->move_vec_.wz = 1;
+      // this->move_vec_.wz = this->wz_dir_mult_;
+      float move_scal_sum = fabs(this->move_vec_.vx) +
+                            fabs(this->move_vec_.vy) + fabs(this->move_vec_.wz);
+      if (move_scal_sum > 1.01f) {
+        this->move_vec_.wz = this->move_vec_.wz / move_scal_sum;
+        this->move_vec_.vx = this->move_vec_.vx / move_scal_sum;
+        this->move_vec_.vy = this->move_vec_.vy / move_scal_sum;
+      }
       break;
     }
     default:
@@ -322,11 +380,11 @@ void OmniChassis<Motor, MotorParam>::Control() {
 
   /* 根据底盘模式计算输出值 */
   switch (this->mode_) {
-    case OmniChassis::BREAK:
-    case OmniChassis::FOLLOW_GIMBAL_INTERSECT:
-    case OmniChassis::FOLLOW_GIMBAL_CROSS:
-    case OmniChassis::ROTOR:
-    case OmniChassis::INDENPENDENT: /* 独立模式,受PID控制 */ {
+    case nOmniChassis::BREAK:
+    case nOmniChassis::FOLLOW_GIMBAL_INTERSECT:
+    case nOmniChassis::FOLLOW_GIMBAL_CROSS:
+    case nOmniChassis::ROTOR:
+    case nOmniChassis::INDENPENDENT: /* 独立模式,受PID控制 */ {
       float percentage = 0.0f;
       if (ref_.status == Device::Referee::RUNNING) {
         if (ref_.chassis_pwr_buff > 30) {
@@ -337,31 +395,40 @@ void OmniChassis<Motor, MotorParam>::Control() {
       } else {
         percentage = 1.0f;
       }
+
       clampf(&percentage, 0.0f, 1.0f);
-      float max_power_limit =
-          ref_.chassis_power_limit +
-          ref_.chassis_power_limit * 0.2 * this->cap_.percentage_;
+
       for (unsigned i = 0; i < this->mixer_.len_; i++) {
         out_.motor3508_out[i] = this->actuator_[i]->Calculate(
             this->setpoint_.motor_rotational_speed[i] *
-                max_motor_rotational_speed_,
+                MOTOR_MAX_ROTATIONAL_SPEED,
             this->motor_[i]->GetSpeed(), this->dt_);
       }
+
+      if (this->cmd_.z > 0.5) {
+        this->max_power_limit = 120;
+      } else {
+        this->max_power_limit = ref_.chassis_power_limit;
+      }
+
       for (unsigned i = 0; i < this->mixer_.len_; i++) {
         if (cap_.online_) {
-          LimitChassisOutPower(max_power_limit, out_.motor3508_out,
-                               motor_feedback_.motor_speed, this->mixer_.len_);
+          // LimitChassisOutPower(this->max_power_limit, out_.motor3508_out,
+          //                      motor_speed_, this->mixer_.len_);
+          LimitChassisOutPower(180, out_.motor3508_out, motor_speed_,
+                               this->mixer_.len_);
           this->motor_[i]->Control(out_.motor3508_out[i]);
         } else {
+          /* 不限功率的兵种使用该底盘时
+           * 注意更改chassis_power_limit至电池安全功率 */
           LimitChassisOutPower(ref_.chassis_power_limit, out_.motor3508_out,
-                               motor_feedback_.motor_speed, this->mixer_.len_);
-
+                               motor_speed_, this->mixer_.len_);
           this->motor_[i]->Control(out_.motor3508_out[i]);
         }
       }
       break;
     }
-    case OmniChassis::RELAX: /* 放松模式,不输出 */
+    case nOmniChassis::RELAX: /* 放松模式,不输出 */
       for (size_t i = 0; i < this->mixer_.len_; i++) {
         this->motor_[i]->Relax();
       }
@@ -373,37 +440,38 @@ void OmniChassis<Motor, MotorParam>::Control() {
 }
 
 template <typename Motor, typename MotorParam>
-void OmniChassis<Motor, MotorParam>::PraseRef() {
+void nOmniChassis<Motor, MotorParam>::PraseRef() {
   this->ref_.chassis_power_limit =
       this->raw_ref_.robot_status.chassis_power_limit;
   this->ref_.chassis_pwr_buff = this->raw_ref_.power_heat.chassis_pwr_buff;
-  this->ref_.chassis_watt = this->raw_ref_.power_heat.chassis_watt;
+  // this->ref_.chassis_watt = this->raw_ref_.power_heat.chassis_watt;
   this->ref_.status = this->raw_ref_.status;
 }
 
+/* 随机转速小陀螺 结合自身超电与敌方视觉水平使用 */
 template <typename Motor, typename MotorParam>
-float OmniChassis<Motor, MotorParam>::CalcWz(const float LO, const float HI) {
+float nOmniChassis<Motor, MotorParam>::CalcWz(const float LO, const float HI) {
   float wz_vary = fabsf(0.2f * sinf(ROTOR_OMEGA * this->now_)) + LO;
   clampf(&wz_vary, LO, HI);
   return wz_vary;
 }
 
 template <typename Motor, typename MotorParam>
-void OmniChassis<Motor, MotorParam>::SetMode(OmniChassis::Mode mode) {
+void nOmniChassis<Motor, MotorParam>::SetMode(nOmniChassis::Mode mode) {
   if (mode == this->mode_) {
     return; /* 模式未改变直接返回 */
   }
 
-  if (mode == OmniChassis::ROTOR && this->mode_ != OmniChassis::ROTOR) {
+  if (mode == nOmniChassis::ROTOR && this->mode_ != nOmniChassis::ROTOR) {
     std::srand(this->now_);
     this->wz_dir_mult_ = (std::rand() % 2) ? -1 : 1;
   }
-  if (mode == OmniChassis::FOLLOW_GIMBAL_CROSS &&
-      this->mode_ != OmniChassis::FOLLOW_GIMBAL_CROSS) {
+  if (mode == nOmniChassis::FOLLOW_GIMBAL_CROSS &&
+      this->mode_ != nOmniChassis::FOLLOW_GIMBAL_CROSS) {
     this->param_.type = Component::Mixer::OMNICROSS;
   }
-  if (mode == OmniChassis::FOLLOW_GIMBAL_INTERSECT &&
-      this->mode_ != OmniChassis::FOLLOW_GIMBAL_INTERSECT) {
+  if (mode == nOmniChassis::FOLLOW_GIMBAL_INTERSECT &&
+      this->mode_ != nOmniChassis::FOLLOW_GIMBAL_INTERSECT) {
     this->param_.type = Component::Mixer::OMNIPLUS;
   }
   /* 切换模式后重置PID和滤波器 */
@@ -413,9 +481,10 @@ void OmniChassis<Motor, MotorParam>::SetMode(OmniChassis::Mode mode) {
   this->mode_ = mode;
 }
 
+/*慢速刷新*/
 template <typename Motor, typename MotorParam>
-void OmniChassis<Motor, MotorParam>::DrawUIStatic(
-    OmniChassis<Motor, MotorParam>* chassis) {
+void nOmniChassis<Motor, MotorParam>::DrawUIStatic(
+    nOmniChassis<Motor, MotorParam>* chassis) {
   chassis->string_.Draw("CM", Component::UI::UI_GRAPHIC_OP_ADD,
                         Component::UI::UI_GRAPHIC_LAYER_CONST,
                         Component::UI::UI_GREEN, UI_DEFAULT_WIDTH * 10, 80,
@@ -474,8 +543,8 @@ void OmniChassis<Motor, MotorParam>::DrawUIStatic(
 }
 
 template <typename Motor, typename MotorParam>
-void OmniChassis<Motor, MotorParam>::DrawUIDynamic(
-    OmniChassis<Motor, MotorParam>* chassis) {
+void nOmniChassis<Motor, MotorParam>::DrawUIDynamic(
+    nOmniChassis<Motor, MotorParam>* chassis) {
   float box_pos_left = 0.0f, box_pos_right = 0.0f;
 
   /* 更新底盘模式选择框 */
@@ -519,4 +588,4 @@ void OmniChassis<Motor, MotorParam>::DrawUIDynamic(
     Device::Referee::AddUI(chassis->rectange_);
   }
 }
-template class Module::OmniChassis<Device::RMMotor, Device::RMMotor::Param>;
+template class Module::nOmniChassis<Device::RMMotor, Device::RMMotor::Param>;
diff --git a/src/module/omni_chassis/mod_omni_chassis.hpp b/src/module/omni_chassis/mod_omni_chassis.hpp
index 38ded65e..81f7a87d 100644
--- a/src/module/omni_chassis/mod_omni_chassis.hpp
+++ b/src/module/omni_chassis/mod_omni_chassis.hpp
@@ -24,7 +24,7 @@
 #include "dev_rm_motor.hpp"
 namespace Module {
 template <typename Motor, typename MotorParam>
-class OmniChassis {
+class nOmniChassis {
  public:
   /* 底盘运行模式 */
   typedef enum {
@@ -36,12 +36,19 @@ class OmniChassis {
     INDENPENDENT, /* 独立模式。底盘运行不受云台影响 */
   } Mode;
 
+  typedef enum {
+    COMMON,
+    BEAST,
+  } Power_Mode;
+
   typedef enum {
     SET_MODE_RELAX,
     SET_MODE_INTERSECT,
     SET_MODE_CROSS,
     SET_MODE_ROTOR,
     SET_MODE_INDENPENDENT,
+    CHANGE_POWER_UP,
+    CHANGE_POWER_DOWN,
   } ChassisEvent;
   /* 底盘参数的结构体,包含所有初始Component化用的参数,通常是const,存好几组
    */
@@ -54,13 +61,15 @@ class OmniChassis {
         Component::Mixer::MECANUM; /* 底盘类型,底盘的机械设计和轮子选型 */
 
     Component::PID::Param follow_pid_param{}; /* 跟随云台PID的参数 */
+    Component::PID::Param xaccl_pid_param{};  /* 加速跟随PID的参数 */
+    Component::PID::Param yaccl_pid_param{};  /* y方向加速跟随PID */
 
     const std::vector<Component::CMD::EventMapItem> EVENT_MAP;
 
     std::array<Component::SpeedActuator::Param, 4> actuator_param{};
 
     std::array<MotorParam, 4> motor_param;
-
+    float (*get_speed)(float);
   } Param;
 
   typedef struct {
@@ -70,7 +79,7 @@ class OmniChassis {
     float chassis_watt;
   } RefForChassis;
 
-  OmniChassis(Param &param, float control_freq);
+  nOmniChassis(Param &param, float control_freq);
 
   void UpdateFeedback();
 
@@ -78,6 +87,8 @@ class OmniChassis {
 
   void SetMode(Mode mode);
 
+  void ChangePowerlim(Power_Mode power_mode_);
+
   bool LimitChassisOutPower(float power_limit, float *motor_out, float *speed,
                             uint32_t len);
   bool LimitChassisOutput(float power_limit, float *motor_out, float *speed,
@@ -85,9 +96,9 @@ class OmniChassis {
 
   void PraseRef();
   uint16_t MAXSPEEDGET(float power_limit);
-  static void DrawUIStatic(OmniChassis<Motor, MotorParam> *chassis);
+  static void DrawUIStatic(nOmniChassis<Motor, MotorParam> *chassis);
 
-  static void DrawUIDynamic(OmniChassis<Motor, MotorParam> *chassis);
+  static void DrawUIDynamic(nOmniChassis<Motor, MotorParam> *chassis);
 
   float CalcWz(const float LO, const float HI);
 
@@ -109,6 +120,8 @@ class OmniChassis {
   RefForChassis ref_;
 
   Mode mode_ = RELAX;
+  Mode last_mode_ = mode_;
+  Power_Mode power_mode_ = COMMON;
 
   Device::Cap::Info cap_;
 
@@ -125,14 +138,19 @@ class OmniChassis {
 
   /* PID计算的目标值 */
   struct {
-    float *motor_rotational_speed; /* 电机转速的动态数组,单位:RPM */
+    float motor_rotational_speed[4]; /* 电机转速的动态数组,单位:RPM */
   } setpoint_;
+  float motor_speed_[4];
   struct {
     float motor3508_out[4]; /*转矩电流范围-1--1*/
   } out_;
   /* 反馈控制用的PID */
 
   Component::PID follow_pid_; /* 跟随云台用的PID */
+  Component::PID xaccl_pid_;  /* x方向加速跟随PID */
+  Component::PID yaccl_pid_;  /* y方向加速跟随PID */
+
+  float max_power_limit;
 
   System::Thread thread_;
 
@@ -141,16 +159,15 @@ class OmniChassis {
   float yaw_;
   Device::Referee::Data raw_ref_;
 
-  Device::BaseMotor::Feedback motor_feedback_;
-
   Component::CMD::ChassisCMD cmd_;
 
   Component::UI::String string_;
+  Component::UI::Cycle cycle_;
 
   Component::UI::Line line_;
 
   Component::UI::Rectangle rectange_;
 };
 
-typedef OmniChassis<Device::RMMotor, Device::RMMotor::Param> RMChassis;
+typedef nOmniChassis<Device::RMMotor, Device::RMMotor::Param> RMChassis;
 }  // namespace Module
diff --git a/src/robot/helm_infantry/.idea/.gitignore b/src/robot/helm_infantry/.idea/.gitignore
new file mode 100644
index 00000000..35410cac
--- /dev/null
+++ b/src/robot/helm_infantry/.idea/.gitignore
@@ -0,0 +1,8 @@
+# 默认忽略的文件
+/shelf/
+/workspace.xml
+# 基于编辑器的 HTTP 客户端请求
+/httpRequests/
+# Datasource local storage ignored files
+/dataSources/
+/dataSources.local.xml
diff --git a/src/robot/helm_infantry/.idea/helm_infantry.iml b/src/robot/helm_infantry/.idea/helm_infantry.iml
new file mode 100644
index 00000000..bc2cd874
--- /dev/null
+++ b/src/robot/helm_infantry/.idea/helm_infantry.iml
@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<module type="CPP_MODULE" version="4">
+  <component name="NewModuleRootManager">
+    <content url="file://$MODULE_DIR$" />
+    <orderEntry type="inheritedJdk" />
+    <orderEntry type="sourceFolder" forTests="false" />
+  </component>
+</module>
\ No newline at end of file
diff --git a/src/robot/helm_infantry/.idea/modules.xml b/src/robot/helm_infantry/.idea/modules.xml
new file mode 100644
index 00000000..106af59b
--- /dev/null
+++ b/src/robot/helm_infantry/.idea/modules.xml
@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ProjectModuleManager">
+    <modules>
+      <module fileurl="file://$PROJECT_DIR$/.idea/helm_infantry.iml" filepath="$PROJECT_DIR$/.idea/helm_infantry.iml" />
+    </modules>
+  </component>
+</project>
\ No newline at end of file
diff --git a/src/robot/helm_infantry/.idea/vcs.xml b/src/robot/helm_infantry/.idea/vcs.xml
new file mode 100644
index 00000000..c2365ab1
--- /dev/null
+++ b/src/robot/helm_infantry/.idea/vcs.xml
@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="VcsDirectoryMappings">
+    <mapping directory="$PROJECT_DIR$/../../.." vcs="Git" />
+  </component>
+</project>
\ No newline at end of file
diff --git a/src/robot/helm_infantry/robot.cpp b/src/robot/helm_infantry/robot.cpp
index f33f88f2..55ff9efa 100644
--- a/src/robot/helm_infantry/robot.cpp
+++ b/src/robot/helm_infantry/robot.cpp
@@ -9,21 +9,36 @@
 /* clang-format off */
 Robot::HelmInfantry::Param param =
     {
+
+    //   .robot_st =
+    //   {
+    //     .k=0.038,//0.092
+    //     .bullet_type=Device::NewAi::BULLET_17,
+    //     .bias_time=60,
+    //     .s_bias=0.21500,
+    //     .z_bias=0.12500,
+    //   },
+
         .chassis =
             {
-                .toque_coefficient_3508 = 0.044876589f,
-                .speed_2_coefficient_3508 = 1.563578147703433e-19f,
-                .out_2_coefficient_3508 = 54.67448580571495f,
-                .constant_3508 = 6.564590719535691f,
+                // .toque_coefficient_3508 = 0.044876589f,
+                // .speed_2_coefficient_3508 = 1.563578147703433e-19f,
+                // .out_2_coefficient_3508 = 54.67448580571495f,
+                // .constant_3508 = 6.564590719535691f,
+                .toque_coefficient_3508 = 0.04585928841670807f,
+                .speed_2_coefficient_3508  = 1.7539151934403435e-18f,
+                .out_2_coefficient_3508 = 26.953338626324133f,
+                .constant_3508 = 4.220175002051428e-09f,
+
 
                 .follow_pid_param =
                 {
-                        .k = 5.0f,
-                        .p = 0.5f,
-                        .i = 0.5f,
-                        .d = 0.0f,
-                        .i_limit = 1.0f,
-                        .out_limit = 5.0f,
+                        .k = 5.f,
+                        .p = 1.3f,
+                        .i = 0.8f,
+                        .d = 0.05f,
+                        .i_limit = 1.2f,
+                        .out_limit = 6.0f,
                         .d_cutoff_freq = -1.0f,
                         .cycle = true,
                 },
@@ -52,25 +67,26 @@ Robot::HelmInfantry::Param param =
                          Module::RMHelmChassis::SET_MODE_6020_FOLLOW}
                          },
 
+
                 .pos_param =
                     {
                         Component::PosActuator::Param{
                             .speed =
                                 {
                                     .k = 0.005f,
-                                    .p = 1.f,
-                                    .i = 0.3f,
-                                    .d = 0.0f,
+                                    .p = 1.0f,//1.0f,
+                                    .i = 0.4f,//0.3f
+                                    .d = 0.001f,
                                     .i_limit = 0.25f,
-                                    .out_limit = 0.4f,
+                                    .out_limit = 0.6f,
                                     .d_cutoff_freq = -1.0f,
                                     .cycle = false,
                                 },
                             .position =
                                 {
                                     .k = 150.0f,
-                                    .p = 1.0f,
-                                    .i = 0.3f,
+                                    .p = 1.5f,
+                                    .i = 0.3f,//0.3f
                                     .d = 0.0f,
                                     .i_limit = 0.0f,
                                     .out_limit = 200.0f,
@@ -86,9 +102,9 @@ Robot::HelmInfantry::Param param =
                             .speed =
                                 {
                                     .k = 0.005f,
-                                    .p = 1.f,
-                                    .i = 0.3f,
-                                    .d = 0.0f,
+                                    .p = 1.0f,//1.0f,
+                                    .i = 0.4f,
+                                    .d = 0.001f,
                                     .i_limit = 0.25f,
                                     .out_limit = 0.4f,
                                     .d_cutoff_freq = -1.0f,
@@ -98,7 +114,7 @@ Robot::HelmInfantry::Param param =
                                 {
                                     .k = 150.0f,
                                     .p = 1.0f,
-                                    .i = 0.3f,
+                                    .i = 0.4f,
                                     .d = 0.0f,
                                     .i_limit = 0.0f,
                                     .out_limit = 200.0f,
@@ -114,9 +130,9 @@ Robot::HelmInfantry::Param param =
                             .speed =
                                 {
                                     .k = 0.005f,
-                                    .p = 1.f,
-                                    .i = 0.3f,
-                                    .d = 0.0f,
+                                    .p = 1.0,//1.0f,
+                                    .i = 0.4f,
+                                    .d = 0.001f,
                                     .i_limit = 0.25f,
                                     .out_limit = 0.4f,
                                     .d_cutoff_freq = -1.0f,
@@ -126,7 +142,7 @@ Robot::HelmInfantry::Param param =
                                 {
                                     .k = 150.0f,
                                     .p = 1.0f,
-                                    .i = 0.3f,
+                                    .i = 0.4f,
                                     .d = 0.0f,
                                     .i_limit = 0.0f,
                                     .out_limit = 200.0f,
@@ -142,9 +158,9 @@ Robot::HelmInfantry::Param param =
                             .speed =
                                 {
                                     .k = 0.005f,
-                                    .p = 1.f,
-                                    .i = 0.3f,
-                                    .d = 0.0f,
+                                    .p = 1.0f,//1.0f,
+                                    .i = 0.4f,//0.3f
+                                    .d = 0.001f,
                                     .i_limit = 0.25f,
                                     .out_limit = 0.4f,
                                     .d_cutoff_freq = -1.0f,
@@ -154,7 +170,7 @@ Robot::HelmInfantry::Param param =
                                 {
                                     .k = 150.0f,
                                     .p = 1.0f,
-                                    .i = 0.3f,
+                                    .i = 0.4f,//0.3f
                                     .d = 0.0f,
                                     .i_limit = 0.0f,
                                     .out_limit = 200.0f,
@@ -172,7 +188,7 @@ Robot::HelmInfantry::Param param =
                         Component::SpeedActuator::Param{
                             .speed =
                                 {
-                                    .k = 0.0001f,
+                                    .k = 0.0001f,//0.0001
                                     .p = 2.0f,
                                     .i = 0.0f,
                                     .d = 0.0f,
@@ -190,7 +206,7 @@ Robot::HelmInfantry::Param param =
                         Component::SpeedActuator::Param{
                             .speed =
                                 {
-                                    .k = 0.0001f,
+                                    .k = 0.0001f,//0.0001
                                     .p = 2.0f,
                                     .i = 0.0f,
                                     .d = 0.0f,
@@ -239,58 +255,57 @@ Robot::HelmInfantry::Param param =
                             .out_cutoff_freq = -1.0f,
                         },
                     },
-
-                .mech_zero = { 1.15201962, 3.88327241, 3.41387439, 0.99171859},
-
+              .mech_zero = {0.448689401,3.6010201,3.14389372,3.13315582},
+              //.mech_zero = {2.10860233,5.35004959,4.736166, 4.7754863},
                 .pos_motor_param =
                     {
                         Device::RMMotor::Param{
-                            .id_feedback = 0x205,
+                            .id_feedback = 0x206,
                             .id_control = GM6020_CTRL_ID_BASE,
                             .model = Device::RMMotor::MOTOR_GM6020,
                             .can = BSP_CAN_2,
-                            .reverse = false
+                            .reverse = false,
                         },
                         Device::RMMotor::Param{
-                            .id_feedback = 0x206,
-                            .id_control = GM6020_CTRL_ID_BASE,
+                            .id_feedback = 0x20A,
+                            .id_control = GM6020_CTRL_ID_EXTAND,
                             .model = Device::RMMotor::MOTOR_GM6020,
                             .can = BSP_CAN_2,
-                            .reverse = false
+                            .reverse = false,
                         },
                         Device::RMMotor::Param{
                             .id_feedback = 0x208,
                             .id_control = GM6020_CTRL_ID_BASE,
                             .model = Device::RMMotor::MOTOR_GM6020,
                             .can = BSP_CAN_2,
-                            .reverse = false
+                            .reverse = false,
                         },
                         Device::RMMotor::Param{
-                            .id_feedback = 0x207,
+                            .id_feedback = 0x205,
                             .id_control = GM6020_CTRL_ID_BASE,
                             .model = Device::RMMotor::MOTOR_GM6020,
                             .can = BSP_CAN_2,
-                            .reverse = false
+                            .reverse = false,
                         },
                     },
                 .speed_motor_param =
                     {
                         Device::RMMotor::Param{
-                            .id_feedback = 0x201,
+                            .id_feedback = 0x203,
                             .id_control = M3508_M2006_CTRL_ID_BASE,
                             .model = Device::RMMotor::MOTOR_M3508,
                             .can = BSP_CAN_1,
                             .reverse = true,
                         },
                         Device::RMMotor::Param{
-                            .id_feedback = 0x202,
+                            .id_feedback = 0x201,
                             .id_control = M3508_M2006_CTRL_ID_BASE,
                             .model = Device::RMMotor::MOTOR_M3508,
                             .can = BSP_CAN_1,
                             .reverse = true,
                         },
                         Device::RMMotor::Param{
-                            .id_feedback = 0x203,
+                            .id_feedback = 0x202,
                             .id_control = M3508_M2006_CTRL_ID_BASE,
                             .model = Device::RMMotor::MOTOR_M3508,
                             .can = BSP_CAN_1,
@@ -364,11 +379,11 @@ Robot::HelmInfantry::Param param =
                         .speed =
                             {
                                 /* GIMBAL_CTRL_PIT_OMEGA_IDX */
-                                .k = 0.4,
-                                .p = 1.0f,
-                                .i = 0.6f,
-                                .d = 0.f,
-                                .i_limit = 0.5f,
+                                .k = 0.4f,
+                                .p = 0.9f,//1.0f,
+                                .i = 0.6f,//0.6f
+                                .d = 0.0f,
+                                .i_limit = 0.7f,
                                 .out_limit = 1.0f,
                                 .d_cutoff_freq = -1.0f,
                                 .cycle = false,
@@ -377,6 +392,39 @@ Robot::HelmInfantry::Param param =
                         .position =
                             {
                                 /* GIMBAL_CTRL_PIT_ANGLE_IDX */
+                                .k = 10.0f,
+                                .p = 1.1f,//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_ai_actr =
+                    {
+                        .speed =
+                            {
+                                /* GIMBAL_CTRL_YAW_OMEGA_IDX */
+                                .k = 0.4f,
+                                .p = 1.f,
+                                .i = 0.5f,
+                                .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,
@@ -389,21 +437,53 @@ Robot::HelmInfantry::Param param =
 
                         .in_cutoff_freq = -1.0f,
 
+                        .out_cutoff_freq = -1.0f,
+                    },
+                .pit_ai_actr =
+                    {
+                        .speed =
+                            {
+                                /* GIMBAL_CTRL_PIT_OMEGA_IDX */
+                                .k = 0.4f,
+                                .p = 0.9f,//1.0f,
+                                .i = 0.6f,//0.6f
+                                .d = 0.0f,
+                                .i_limit = 0.7f,
+                                .out_limit = 1.0f,
+                                .d_cutoff_freq = -1.0f,
+                                .cycle = false,
+                            },
+
+                        .position =
+                            {
+                                /* GIMBAL_CTRL_PIT_ANGLE_IDX */
+                                .k = 10.0f,
+                                .p = 1.1f,//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 = 0x20A,
+                        .id_feedback = 0x209,
                         .id_control = GM6020_CTRL_ID_EXTAND,
                         .model = Device::RMMotor::MOTOR_GM6020,
                         .can = BSP_CAN_2,
-                        .reverse = false
+                        .reverse=false,
                     },
 
                 .pit_motor =
                     {
-                        .id_feedback = 0x209,
+                        .id_feedback = 0x20B,
                         .id_control = GM6020_CTRL_ID_EXTAND,
                         .model = Device::RMMotor::MOTOR_GM6020,
                         .can = BSP_CAN_2,
@@ -412,15 +492,16 @@ Robot::HelmInfantry::Param param =
 
                 .mech_zero =
                     {
-                        .yaw = 6.26477766f,
-                        .pit = M_2PI - 1.0f,
+                        //.yaw = 2.08314586f,
+                        .yaw = 3.66237926f,
+                        .pit = M_2PI - 5.4602046f,
                         .rol = 0.0f,
                     },
 
                 .limit =
                     {
-                        .pitch_max = M_2PI - 0.60f,
-                        .pitch_min = M_2PI - 1.4f,
+                        .pitch_max = M_2PI - 5.1f,
+                        .pitch_min = M_2PI - 5.8f,
                         .yaw_max = 0.0f,
                         .yaw_min = 0.0f,
                     },
@@ -428,32 +509,37 @@ Robot::HelmInfantry::Param param =
                 .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::ABSOLUTE},
                               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::START_AUTO_AIM},
+                                  Module::Gimbal::SET_MODE_ABSOLUTE},
                               Component::CMD::EventMapItem{
                                   Device::DR16::DR16_SW_R_POS_BOT,
-                                  Module::Gimbal::START_AUTO_AIM},
+                                  Module::Gimbal::SET_MODE_ABSOLUTE},
                               Component::CMD::EventMapItem{
                                   Device::DR16::KEY_R_PRESS,
-                                  Module::Gimbal::START_AUTO_AIM},
+                                  Module::Gimbal::SET_MODE_AUTO_AIM},
                               Component::CMD::EventMapItem{
                                   Device::DR16::KEY_R_RELEASE,
-                                  Module::Gimbal::STOP_AUTO_AIM}},
+                                  Module::Gimbal::SET_MODE_ABSOLUTE}},
 
             },
     .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 = 30.f,
+    .num_trig_tooth = 10.0f,
+    .trig_gear_ratio = 36.0f * 2.5f,
+    .fric_radius = 0.03f,
+    .cover_open_duty = 0.125f,
+    .cover_close_duty = 0.075f,
+    .model = Module::RMLauncher::LAUNCHER_MODEL_17MM,
+    .default_bullet_speed = 25.0f,
         .min_launch_delay = static_cast<uint32_t>(1000.0f / 20.0f),
+        // .allow_reverse = false,
+        // .fric_speed = 3000.0f,
 
         .trig_actr = {
             Component::PosActuator::Param{
@@ -488,8 +574,8 @@ Robot::HelmInfantry::Param param =
         .fric_actr = {
            Component::SpeedActuator::Param{
             .speed = {
-            .k = 0.00025f,
-            .p = 1.0f,
+            .k = 0.0003f,
+            .p = 1.8f,
             .i = 0.0f,
             .d = 0.0f,
             .i_limit = 0.2f,
@@ -504,8 +590,8 @@ Robot::HelmInfantry::Param param =
             },
            Component::SpeedActuator::Param{
             .speed = {
-            .k = 0.00025f,
-            .p = 1.0f,
+            .k = 0.0003f,
+            .p = 1.8f,
             .i = 0.0f,
             .d = 0.0f,
             .i_limit = 0.2f,
@@ -520,7 +606,7 @@ Robot::HelmInfantry::Param param =
         },
         },
 
-        .trig_motor = {
+        .trig_param = {
         Device::RMMotor::Param{
             .id_feedback = 0x204,
             .id_control = M3508_M2006_CTRL_ID_BASE,
@@ -530,60 +616,65 @@ Robot::HelmInfantry::Param param =
         }
         },
 
-        .fric_motor = {
+        .fric_param = {
         Device::RMMotor::Param{
             .id_feedback = 0x206,
             .id_control = M3508_M2006_CTRL_ID_EXTAND,
             .model = Device::RMMotor::MOTOR_M3508,
             .can = BSP_CAN_1,
-            .reverse = false
+            .reverse=false,
         },
         Device::RMMotor::Param{
             .id_feedback = 0x207,
             .id_control = M3508_M2006_CTRL_ID_EXTAND,
             .model = Device::RMMotor::MOTOR_M3508,
             .can = BSP_CAN_1,
-            .reverse = false
+            .reverse=false,
         },
         },
 
         .EVENT_MAP = {
         Component::CMD::EventMapItem{
             Component::CMD::CMD_EVENT_LOST_CTRL,
-            Module::Launcher::CHANGE_FIRE_MODE_RELAX
+            Module::RMLauncher::CHANGE_FIRE_MODE_RELAX
         },
         Component::CMD::EventMapItem{
             Device::DR16::DR16_SW_R_POS_TOP,
-            Module::Launcher::CHANGE_FIRE_MODE_SAFE
+            Module::RMLauncher::CHANGE_FIRE_MODE_SAFE
         },
         Component::CMD::EventMapItem{
             Device::DR16::DR16_SW_R_POS_MID,
-            Module::Launcher::CHANGE_FIRE_MODE_LOADED
+            Module::RMLauncher::CHANGE_FIRE_MODE_LOADED
         },
+        // Component::CMD::EventMapItem{
+        //     Device::DR16::DR16_SW_R_POS_MID,
+        //     Module::Launcher::LAUNCHER_STOP_TRIG
+        // },
         Component::CMD::EventMapItem{
             Device::DR16::DR16_SW_R_POS_BOT,
-            Module::Launcher::CHANGE_FIRE_MODE_LOADED
+            Module::RMLauncher::CHANGE_FIRE_MODE_LOADED,
         },
         Component::CMD::EventMapItem{
             Device::DR16::DR16_SW_R_POS_BOT,
-            Module::Launcher::LAUNCHER_START_FIRE
+            Module::RMLauncher::LAUNCHER_START_FIRE,
         },
+        // Component::CMD::EventMapItem{
+        //     Device::DR16::DR16_SW_R_POS_BOT,
+        //     Module::Launcher::CHANGE_TRIG_MODE_BURST,
+        // },
         Component::CMD::EventMapItem{
             Device::DR16::KEY_L_PRESS,
-            Module::Launcher::LAUNCHER_START_FIRE
+            Module::RMLauncher::CHANGE_TRIG_MODE_BURST
         },
         Component::CMD::EventMapItem{
-            Device::DR16::KEY_G,
-            Module::Launcher::CHANGE_TRIG_MODE
+        Device::DR16::KEY_L_RELEASE,
+        Module::RMLauncher::LAUNCHER_STOP_TRIG
         },
         Component::CMD::EventMapItem{
-            Device::DR16::KEY_R,
-            Module::Launcher::OPEN_COVER
+            Device::DR16::KEY_G,
+            Module::RMLauncher::CHANGE_TRIG_MODE
         },
-        Component::CMD::EventMapItem{
-            Device::DR16::KEY_F,
-            Module::Launcher::CLOSE_COVER
-        }
+
     },
   }, /* launcher */
   .bmi088_rot =
@@ -596,7 +687,7 @@ Robot::HelmInfantry::Param param =
                     },
             },
   .cap = {
-    .can = BSP_CAN_1,
+    .can = BSP_CAN_2,
   },
 };
 /* clang-format on */
diff --git a/src/robot/helm_infantry/robot.hpp b/src/robot/helm_infantry/robot.hpp
index 201b620c..af06f7bb 100644
--- a/src/robot/helm_infantry/robot.hpp
+++ b/src/robot/helm_infantry/robot.hpp
@@ -1,7 +1,7 @@
 /* #include "dev_xxx.hpp" */
 #include "comp_cmd.hpp"
 #include "dev_ahrs.hpp"
-#include "dev_ai.hpp"
+#include "dev_aim.hpp"
 #include "dev_bmi088.hpp"
 #include "dev_can.hpp"
 #include "dev_cap.hpp"
@@ -20,7 +20,7 @@ class HelmInfantry {
   typedef struct Param {
     Module::RMHelmChassis::Param chassis;
     Module::Gimbal::Param gimbal;
-    Module::Launcher::Param launcher;
+    Module::RMLauncher::Param launcher;
     Device::BMI088::Rotation bmi088_rot{};
     Device::Cap::Param cap{};
   } Param;
@@ -29,7 +29,7 @@ class HelmInfantry {
 
   Device::Referee referee_;
   Device::Can can_;
-  Device::AI ai_;
+  Device::AIM aim_;
   Device::AHRS ahrs_;
   Device::BMI088 bmi088_;
   Device::Cap cap_;
@@ -38,7 +38,7 @@ class HelmInfantry {
 
   Module::RMHelmChassis chassis_;
   Module::Gimbal gimbal_;
-  Module::Launcher launcher_;
+  Module::RMLauncher launcher_;
 
   HelmInfantry(Param& param, float control_freq)
       : bmi088_(param.bmi088_rot),
diff --git a/src/robot/omni_infantry/robot.cpp b/src/robot/omni_infantry/robot.cpp
index 976cd2a9..06abdab0 100644
--- a/src/robot/omni_infantry/robot.cpp
+++ b/src/robot/omni_infantry/robot.cpp
@@ -3,36 +3,64 @@
 #include <comp_actuator.hpp>
 
 #include "dev_rm_motor.hpp"
-#include "mod_omni_chassis.hpp"
+// #include "mod_new_omni_chassis.hpp"
 #include "system.hpp"
 
 /* clang-format off */
 Robot::OmniInfantry::Param param = {
     .chassis={
-      .toque_coefficient_ = 0.0327120418848f,
-      .speed_2_coefficient_ = 1.227822928729637e-07,
-      .out_2_coefficient_ = 1.1108430132455055e-24,
-      .constant_ = 1.8135014050213443,
+      .toque_coefficient_ = 0.03985309056810189f,
+      .speed_2_coefficient_  = 5.297566618742181e-08f,
+      .out_2_coefficient_ = 28.851126052234243f,
+      .constant_ = 7.479591458247737f,
 
       .follow_pid_param = {
-      .k = 0.5f,
-      .p = 1.0f,
-      .i = 0.0f,
-      .d = 0.0f,
-      .i_limit = 1.0f,
+      .k = -1.5f,
+      //.p = 0.45f,
+      .p = 0.70f,
+      .i = 0.05f,
+      .d = 0.01f,
+      .i_limit = 0.5f,
       .out_limit = 1.0f,
       .d_cutoff_freq = -1.0f,
       .cycle = true,
     },
 
+      .xaccl_pid_param =
+      {
+      .k = 1.0f,
+      .p = 0.6f,
+      .i = 1.6f,
+      .d = 0.00f,
+      .i_limit = 1.0f,
+      .out_limit = 1.0f,
+      .d_cutoff_freq = -0.001f,
+      .cycle = false,
+      },
+      .yaccl_pid_param =
+      {
+      .k = 1.0f,
+      .p = 0.6f,
+      .i = 1.6f,
+      .d = 0.00f,
+      .i_limit = 1.0f,
+      .out_limit = 1.0f,
+      .d_cutoff_freq = -0.001f,
+      .cycle = false,
+      },
+
     .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_TOP,
-        Module::RMChassis::SET_MODE_RELAX
+        Module::RMChassis::RELAX
       },
       Component::CMD::EventMapItem{
         Device::DR16::DR16_SW_L_POS_MID,
@@ -59,6 +87,7 @@ Robot::OmniInfantry::Param param = {
     .actuator_param = {
       Component::SpeedActuator::Param{
         .speed = {
+          //.k = 0.00050f,
           .k = 0.00015f,
           .p = 1.0f,
           .i = 0.0f,
@@ -76,7 +105,7 @@ Robot::OmniInfantry::Param param = {
       },
       Component::SpeedActuator::Param{
         .speed = {
-          .k = 0.00018f,
+          .k = 0.00015f,
           .p = 1.0f,
           .i = 0.0f,
           .d = 0.0f,
@@ -130,26 +159,47 @@ Robot::OmniInfantry::Param param = {
           .id_control = M3508_M2006_CTRL_ID_BASE,
           .model = Device::RMMotor::MOTOR_M3508,
           .can = BSP_CAN_1,
+          .reverse=false,
       },
       Device::RMMotor::Param{
           .id_feedback = 0x204,
           .id_control = M3508_M2006_CTRL_ID_BASE,
           .model = Device::RMMotor::MOTOR_M3508,
           .can = BSP_CAN_1,
+          .reverse=false,
       },
       Device::RMMotor::Param{
           .id_feedback = 0x201,
           .id_control = M3508_M2006_CTRL_ID_BASE,
           .model = Device::RMMotor::MOTOR_M3508,
           .can = BSP_CAN_1,
+          .reverse=false,
       },
       Device::RMMotor::Param{
           .id_feedback = 0x202,
           .id_control = M3508_M2006_CTRL_ID_BASE,
           .model = Device::RMMotor::MOTOR_M3508,
           .can = BSP_CAN_1,
+          .reverse=false,
       },
     },
+      .get_speed = [](float power_limit){
+      float speed = 0.0f;
+      if (power_limit <= 50.0f) {
+        speed = 5500;
+      } else if (power_limit <= 60.0f) {
+        speed = 6000;
+      } else if (power_limit <= 70.0f) {
+        speed = 6500;
+      } else if (power_limit <= 80.0f) {
+        speed = 7000;
+      } else if (power_limit <= 100.0f) {
+        speed = 7500;
+      } else {
+        speed = 8000;
+      }
+      return speed;
+    },
   },
 
   .gimbal= {
@@ -174,9 +224,9 @@ Robot::OmniInfantry::Param param = {
     .yaw_actr = {
       .speed = {
           /* GIMBAL_CTRL_YAW_OMEGA_IDX */
-          .k = 0.28f,
+          .k = 0.4f,
           .p = 1.f,
-          .i = 1.f,
+          .i = 0.5f,
           .d = 0.f,
           .i_limit = 0.2f,
           .out_limit = 1.0f,
@@ -201,21 +251,50 @@ Robot::OmniInfantry::Param param = {
         .out_cutoff_freq = -1.0f,
     },
     .pit_actr = {
-        .speed = {
-          /* GIMBAL_CTRL_PIT_OMEGA_IDX */
-          .k = 0.0f,
-          .p = 0.0f,
-          .i = 0.f,
+      .speed = {
+          /* GIMBAL_CTRL_YAW_OMEGA_IDX */
+          .k = 0.4f,
+          .p = 0.8f,
+          .i = 0.1f,
+          .d = 0.0f,
+          .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 = 0.75f,
+          .i = 0.1f,
+          .d = 0.0f,
+          .i_limit = 0.2f,
+          .out_limit = 10.0f,
+          .d_cutoff_freq = -1.0f,
+          .cycle = true,
+        },
+
+        .in_cutoff_freq = -1.0f,
+
+        .out_cutoff_freq = -1.0f,
+    },
+        .yaw_ai_actr = {
+      .speed = {
+          /* GIMBAL_CTRL_YAW_OMEGA_IDX */
+          .k = 0.4f,
+          .p = 1.f,
+          .i = 0.5f,
           .d = 0.f,
-          .i_limit = 0.f,
-          .out_limit = 0.0f,
+          .i_limit = 0.2f,
+          .out_limit = 1.0f,
           .d_cutoff_freq = -1.0f,
           .cycle = false,
         },
 
         .position = {
-          /* GIMBAL_CTRL_PIT_ANGLE_IDX */
-          .k = 30.0f,
+          /* GIMBAL_CTRL_YAW_ANGLE_IDX */
+          .k = 20.0f,
           .p = 1.0f,
           .i = 0.0f,
           .d = 0.0f,
@@ -229,29 +308,60 @@ Robot::OmniInfantry::Param param = {
 
         .out_cutoff_freq = -1.0f,
     },
+    .pit_ai_actr = {
+      .speed = {
+          /* GIMBAL_CTRL_YAW_OMEGA_IDX */
+          .k = 0.4f,
+          .p = 0.8f,
+          .i = 0.1f,
+          .d = 0.0f,
+          .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 = 0.75f,
+          .i = 0.1f,
+          .d = 0.0f,
+          .i_limit = 0.2f,
+          .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 = 0x205,
       .id_control = GM6020_CTRL_ID_BASE,
       .model = Device::RMMotor::MOTOR_GM6020,
       .can = BSP_CAN_1,
+      .reverse = true,
     },
     .pit_motor ={
-      .kp = 10.0f,
-      .kd = 1.0f,
-      .feedback_id = 0,
-      .id = 1,
+      .id_feedback = 0x207,
+      .id_control = GM6020_CTRL_ID_BASE,
+      .model = Device::RMMotor::MOTOR_GM6020,
       .can = BSP_CAN_2,
       .reverse = true,
     },
     .mech_zero = {
-      .yaw = 1.3f + M_PI / 2.0f,
-      .pit = 0.0f,
+      .yaw = M_2PI - 3.69229198f,
+      .pit = 5.82529211,
       .rol = 0.0f,
     },
 
     .limit = {
-      .pitch_max = M_2PI - 0.38f,
-      .pitch_min = M_2PI - 2.27f,
+      .pitch_max = M_2PI - 5.49569994f,
+      .pitch_min = M_2PI - 6.15663195f,
+      .yaw_max = 0.0f,
+      .yaw_min = 0.0f,
     },
 
     .EVENT_MAP = {
@@ -259,6 +369,10 @@ Robot::OmniInfantry::Param param = {
         Component::CMD::CMD_EVENT_LOST_CTRL,
         Module::Gimbal::SET_MODE_RELAX
       },
+      Component::CMD::EventMapItem{
+        Device::DR16::DR16_SW_R_POS_TOP,
+        Module::Gimbal::ABSOLUTE
+      },
       Component::CMD::EventMapItem{
         Device::DR16::DR16_SW_R_POS_TOP,
         Module::Gimbal::SET_MODE_ABSOLUTE
@@ -269,50 +383,57 @@ Robot::OmniInfantry::Param param = {
       },
       Component::CMD::EventMapItem{
         Device::DR16::DR16_SW_R_POS_BOT,
-        Module::Gimbal::SET_MODE_ABSOLUTE
-      },
+        Module::Gimbal::SET_MODE_AUTO_AIM
+      },
+      // Component::CMD::EventMapItem{
+      //   Device::DR16::DR16_SW_R_POS_MID,
+      //   Module::Gimbal::ABSOLUTE
+      // },
+      // Component::CMD::EventMapItem{
+      //   Device::DR16::DR16_SW_R_POS_BOT,
+      //   Module::Gimbal::ABSOLUTE
+      // },
       Component::CMD::EventMapItem{
         Device::DR16::KEY_R_PRESS,
-        Module::Gimbal::START_AUTO_AIM
+        Module::Gimbal::SET_MODE_AUTO_AIM
       },
       Component::CMD::EventMapItem{
         Device::DR16::KEY_R_RELEASE,
-        Module::Gimbal::STOP_AUTO_AIM
+        Module::Gimbal::SET_MODE_ABSOLUTE
       }
     },
-
   },
 
   .launcher = {
-    .num_trig_tooth = 8.0f,
-    .trig_gear_ratio = 36.0f,
+    .num_trig_tooth = 10.0f,
+    .trig_gear_ratio = 36.0f * 2.5f,
     .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),
+    .model = Module::RMLauncher::LAUNCHER_MODEL_17MM,
+    .default_bullet_speed = 25.0f,
+    .min_launch_delay = static_cast<uint32_t>(1000.0f / 12.0f),
 
     .trig_actr = {
       Component::PosActuator::Param{
         .speed = {
-          .k = 5.0f,
-          .p = 1.0f,
-          .i = 0.15f,
+          .k = 3.0f,
+          .p = 2.0f,
+          .i = 0.5f,
           .d = 0.0f,
-          .i_limit = 0.1f,
-          .out_limit = 1.5f,
+          .i_limit = 0.5f,
+          .out_limit = 1.0f,
           .d_cutoff_freq = -1.0f,
           .cycle = false,
         },
 
         .position = {
-          .k = 5.0f,
+          .k = 1.8f,
           .p = 1.0f,
           .i = 0.0f,
-          .d = 0.010f,
+          .d = 0.0f,
           .i_limit = 0.0f,
-          .out_limit = 0.70f,
+          .out_limit = 0.55f,
           .d_cutoff_freq = -1.0f,
           .cycle = true,
         },
@@ -323,109 +444,128 @@ Robot::OmniInfantry::Param param = {
       },
     },
 
-    .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,
+        .fric_actr = {
+           Component::SpeedActuator::Param{
+            .speed = {
+            .k = 0.00025f,
+            .p = 1.0f,
+            .i = 0.0f,
+            .d = 0.0f,
+            .i_limit = 0.2f,
+            .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.0f,
+            .d = 0.0f,
+            .i_limit = 0.2f,
+            .out_limit = 1.0f,
+            .d_cutoff_freq = -1.0f,
+            .cycle = false,
+            },
+
+            .in_cutoff_freq = -1.0f,
+
+            .out_cutoff_freq = -1.0f,
         },
-
-        .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 = {
+    .trig_param = {
       Device::RMMotor::Param{
-        .id_feedback = 0x208,
+        .id_feedback = 0x207,
         .id_control = M3508_M2006_CTRL_ID_EXTAND,
         .model = Device::RMMotor::MOTOR_M2006,
-        .can = BSP_CAN_2,
+        .can = BSP_CAN_1,
+        .reverse = true,
       }
     },
 
-    .fric_motor = {
+    .fric_param = {
       Device::RMMotor::Param{
-          .id_feedback = 0x206,
-          .id_control = M3508_M2006_CTRL_ID_EXTAND,
+          .id_feedback = 0x201,
+          .id_control = M3508_M2006_CTRL_ID_BASE,
           .model = Device::RMMotor::MOTOR_M3508,
           .can = BSP_CAN_2,
+          .reverse=false,
       },
       Device::RMMotor::Param{
-          .id_feedback = 0x205,
-          .id_control = M3508_M2006_CTRL_ID_EXTAND,
+          .id_feedback = 0x203,
+          .id_control = M3508_M2006_CTRL_ID_BASE,
           .model = Device::RMMotor::MOTOR_M3508,
           .can = BSP_CAN_2,
+          .reverse=false,
       },
     },
 
     .EVENT_MAP = {
       Component::CMD::EventMapItem{
         Component::CMD::CMD_EVENT_LOST_CTRL,
-        Module::Launcher::CHANGE_FIRE_MODE_RELAX
+        Module::RMLauncher::CHANGE_FIRE_MODE_RELAX
       },
       Component::CMD::EventMapItem{
         Device::DR16::DR16_SW_R_POS_TOP,
-        Module::Launcher::CHANGE_FIRE_MODE_SAFE
+        Module::RMLauncher::CHANGE_FIRE_MODE_SAFE
       },
       Component::CMD::EventMapItem{
         Device::DR16::DR16_SW_R_POS_MID,
-        Module::Launcher::CHANGE_FIRE_MODE_LOADED
+        Module::RMLauncher::CHANGE_FIRE_MODE_LOADED
+      },
+      Component::CMD::EventMapItem{
+        Device::DR16::DR16_SW_R_POS_MID,
+        Module::RMLauncher::LAUNCHER_STOP_TRIG
       },
       Component::CMD::EventMapItem{
         Device::DR16::DR16_SW_R_POS_BOT,
-        Module::Launcher::CHANGE_FIRE_MODE_LOADED
+        Module::RMLauncher::CHANGE_FIRE_MODE_LOADED
       },
+      // Component::CMD::EventMapItem{
+      //   Device::DR16::DR16_SW_R_POS_BOT,
+      //   Module::RMLauncher::LAUNCHER_START_FIRE
+      // },
       Component::CMD::EventMapItem{
         Device::DR16::DR16_SW_R_POS_BOT,
-        Module::Launcher::LAUNCHER_START_FIRE
+        Module::RMLauncher::CHANGE_TRIG_MODE_BURST
       },
       Component::CMD::EventMapItem{
         Device::DR16::KEY_L_PRESS,
-        Module::Launcher::LAUNCHER_START_FIRE
+        Module::RMLauncher::CHANGE_TRIG_MODE_BURST
+      },
+      Component::CMD::EventMapItem{
+        Device::DR16::KEY_L_RELEASE,
+        Module::RMLauncher::LAUNCHER_STOP_TRIG
       },
       Component::CMD::EventMapItem{
         Device::DR16::KEY_G,
-        Module::Launcher::CHANGE_TRIG_MODE
+        Module::RMLauncher::CHANGE_TRIG_MODE
       },
       Component::CMD::EventMapItem{
         Device::DR16::KEY_R,
-        Module::Launcher::OPEN_COVER
+        Module::RMLauncher::OPEN_COVER
       },
       Component::CMD::EventMapItem{
         Device::DR16::KEY_F,
-        Module::Launcher::CLOSE_COVER
-      }
+        Module::RMLauncher::CLOSE_COVER
+      },
+      // Component::CMD::EventMapItem{
+      //   Device::DR16::KEY_V,
+      // }
+      //   Module::RMLauncher::CHANGE_AI_MODE
     },
   }, /* launcher */
 
   .bmi088_rot = {
     .rot_mat = {
-      { +1, +0, +0},
       { +0, +1, +0},
+      { -1, +0, +0},
       { +0, +0, +1},
     },
   },
diff --git a/src/robot/omni_infantry/robot.hpp b/src/robot/omni_infantry/robot.hpp
index 97f0f8df..0064a3ea 100644
--- a/src/robot/omni_infantry/robot.hpp
+++ b/src/robot/omni_infantry/robot.hpp
@@ -1,15 +1,15 @@
 #include "comp_cmd.hpp"
 #include "dev_ahrs.hpp"
-#include "dev_ai.hpp"
+#include "dev_aim.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_gimbal.hpp"
 #include "mod_launcher.hpp"
 #include "mod_omni_chassis.hpp"
-#include "mod_omni_gimbal.hpp"
 
 void robot_init();
 namespace Robot {
@@ -18,14 +18,14 @@ class OmniInfantry {
   typedef struct Param {
     Module::RMChassis::Param chassis;
     Module::Gimbal::Param gimbal;
-    Module::Launcher::Param launcher;
+    Module::RMLauncher::Param launcher;
     Device::BMI088::Rotation bmi088_rot{};
     Device::Cap::Param cap{};
   } Param;
 
   Component::CMD cmd_;
 
-  Device::AI ai_;
+  Device::AIM ai_;
   Device::AHRS ahrs_;
   Device::BMI088 bmi088_;
   Device::Can can_;
@@ -36,7 +36,7 @@ class OmniInfantry {
 
   Module::RMChassis chassis_;
   Module::Gimbal gimbal_;
-  Module::Launcher launcher_;
+  Module::RMLauncher launcher_;
 
   OmniInfantry(Param& param, float control_freq)
       : bmi088_(param.bmi088_rot),