diff --git a/CMakeLists.txt b/CMakeLists.txt
index a6436df..ac1c33a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -111,4 +111,10 @@ target_link_libraries(uav ${OpenCV_LIBS} fmt::fmt yaml-cpp auto_aim auto_buff co
 add_executable(uav_debug src/task/uav_debug.cpp)
 target_link_libraries(uav_debug ${OpenCV_LIBS} fmt::fmt yaml-cpp nlohmann_json::nlohmann_json auto_aim auto_buff component device)
 
+add_executable(balance_infantry src/task/balance_infantry.cpp)
+target_link_libraries(balance_infantry ${OpenCV_LIBS} fmt::fmt yaml-cpp nlohmann_json::nlohmann_json auto_aim auto_buff component device)
+
+add_executable(balance_infantry_mpc src/task/balance_infantry_mpc.cpp)
+target_link_libraries(balance_infantry_mpc ${OpenCV_LIBS} fmt::fmt yaml-cpp nlohmann_json::nlohmann_json auto_aim auto_buff component device)
+
 
diff --git a/src/task/README.md b/src/task/README.md
new file mode 100644
index 0000000..1ddc12e
--- /dev/null
+++ b/src/task/README.md
@@ -0,0 +1,70 @@
+# Task 目录说明
+
+本目录存放各兵种的主程序，`test/` 子目录存放测试用例。
+
+## 主程序
+
+### 步兵（Standard）
+
+| 文件 | 设备 | 线程 | 说明 |
+|------|------|------|------|
+| `standard.cpp` | CBoard (CAN) | 单线程 | 基础步兵自瞄，仅发送 yaw/pitch 位置指令 |
+| `standard_mpc.cpp` | Gimbal (串口) | 多线程 | MPC 规划步兵，发送 pos/vel/acc，支持自瞄 + 打符 |
+| `mt_standard.cpp` | CBoard (CAN) | 多线程 | 多线程步兵，检测与主循环分离，支持自瞄 + 打符 |
+
+```bash
+./build/standard configs/standard3.yaml
+./build/standard_mpc configs/standard4.yaml
+./build/mt_standard configs/standard3.yaml
+```
+
+### 无人机（UAV）
+
+| 文件 | 设备 | 线程 | 说明 |
+|------|------|------|------|
+| `uav.cpp` | CBoard (CAN) | 单线程 | 无人机自瞄 + 打符，按模式切换 |
+| `uav_debug.cpp` | CBoard (CAN) | 单线程 | 无人机调试版，含重投影可视化和 PlotJuggler 数据输出 |
+
+```bash
+./build/uav configs/uav.yaml
+./build/uav_debug configs/uav.yaml
+```
+
+### 哨兵（Sentry）
+
+| 文件 | 设备 | 线程 | 说明 |
+|------|------|------|------|
+| `sentry.cpp` | CBoard (CAN) | 单线程 | 哨兵基础版，全向感知 + ROS2 通信 |
+| `sentry_multithread.cpp` | CBoard (CAN) | 多线程 | 哨兵多线程版，全向感知 + ROS2 |
+| `sentry_bp.cpp` | CBoard (CAN) | 单线程 | 哨兵 + 后置摄像头，全向感知 + ROS2 |
+| `sentry_debug.cpp` | CBoard (CAN) | 单线程 | 哨兵调试版，含可视化 + ROS2 |
+
+```bash
+./build/sentry configs/sentry.yaml
+./build/sentry_multithread configs/sentry.yaml
+```
+
+### 调试程序（Debug）
+
+| 文件 | 设备 | 线程 | 说明 |
+|------|------|------|------|
+| `auto_aim_debug_mpc.cpp` | Gimbal (串口) | 多线程 | 自瞄 MPC 调试，含重投影可视化和 PlotJuggler 数据 |
+| `mt_auto_aim_debug.cpp` | CBoard (CAN) | 多线程 | 多线程自瞄调试，含可视化，支持自瞄 + 打符 |
+| `auto_buff_debug.cpp` | CBoard (CAN) | 单线程 | 打符调试，含可视化和 PlotJuggler 数据 |
+| `auto_buff_debug_mpc.cpp` | Gimbal (串口) | 单线程 | 打符 MPC 调试，含可视化和 PlotJuggler 数据 |
+
+```bash
+./build/auto_aim_debug_mpc configs/sentry.yaml
+./build/mt_auto_aim_debug configs/sentry.yaml
+./build/auto_buff_debug configs/uav.yaml
+./build/auto_buff_debug_mpc configs/uav.yaml
+```
+
+## 设备对比
+
+- CBoard (CAN)：通过 CAN 总线通信，发送 `Command{control, shoot, yaw, pitch}`，适用于 C 板下位机
+- Gimbal (串口)：通过 USB 串口通信，发送 `pos/vel/acc`，配合 MPC 规划器使用，适用于达妙 MC02
+
+## 测试用例
+
+测试用例位于 `test/` 子目录，用于单独验证各模块功能，详见 [test/](test/) 目录。
diff --git a/src/task/balance_infantry.cpp b/src/task/balance_infantry.cpp
new file mode 100644
index 0000000..861ddc0
--- /dev/null
+++ b/src/task/balance_infantry.cpp
@@ -0,0 +1,120 @@
+#include <fmt/core.h>
+
+#include <chrono>
+#include <nlohmann/json.hpp>
+#include <opencv2/opencv.hpp>
+
+#include "src/device/camera.hpp"
+#include "src/device/cboard.hpp"
+#include "src/module/auto_aim/aimer.hpp"
+#include "src/module/auto_aim/shooter.hpp"
+#include "src/module/auto_aim/solver.hpp"
+#include "src/module/auto_aim/tracker.hpp"
+#include "src/module/auto_aim/yolo.hpp"
+#include "src/module/auto_buff/buff_aimer.hpp"
+#include "src/module/auto_buff/buff_detector.hpp"
+#include "src/module/auto_buff/buff_solver.hpp"
+#include "src/module/auto_buff/buff_target.hpp"
+#include "src/module/auto_buff/buff_type.hpp"
+#include "src/component/exiter.hpp"
+#include "src/component/img_tools.hpp"
+#include "src/component/logger.hpp"
+#include "src/component/math_tools.hpp"
+#include "src/component/plotter.hpp"
+#include "src/component/recorder.hpp"
+
+using namespace std::chrono;
+
+const std::string keys =
+  "{help h usage ? |      | 输出命令行参数说明}"
+  "{@config-path   | configs/standard4.yaml | 位置参数，yaml配置文件路径 }";
+
+int main(int argc, char * argv[])
+{
+  cv::CommandLineParser cli(argc, argv, keys);
+  auto config_path = cli.get<std::string>(0);
+  if (cli.has("help") || config_path.empty()) {
+    cli.printMessage();
+    return 0;
+  }
+
+  component::Exiter exiter;
+  component::Plotter plotter;
+  component::Recorder recorder;
+
+  device::CBoard cboard(config_path);
+  device::Camera camera(config_path);
+
+  auto_aim::YOLO detector(config_path, false);
+  auto_aim::Solver solver(config_path);
+  auto_aim::Tracker tracker(config_path, solver);
+  auto_aim::Aimer aimer(config_path);
+  auto_aim::Shooter shooter(config_path);
+
+  auto_buff::Buff_Detector buff_detector(config_path);
+  auto_buff::Solver buff_solver(config_path);
+  auto_buff::SmallTarget buff_small_target;
+  auto_buff::BigTarget buff_big_target;
+  auto_buff::Aimer buff_aimer(config_path);
+
+  cv::Mat img;
+  Eigen::Quaterniond q;
+  std::chrono::steady_clock::time_point t;
+
+  auto mode = device::Mode::idle;
+  auto last_mode = device::Mode::idle;
+
+  while (!exiter.exit()) {
+    camera.read(img, t);
+    q = cboard.imu_at(t - 1ms);
+    mode = cboard.mode;
+
+    if (last_mode != mode) {
+      component::logger()->info("Switch to {}", device::MODES[mode]);
+      last_mode = mode;
+    }
+
+    solver.set_R_gimbal2world(q);
+
+    Eigen::Vector3d ypr = component::eulers(solver.R_gimbal2world(), 2, 1, 0);
+
+    /// 自瞄
+    if (mode == device::Mode::auto_aim || mode == device::Mode::outpost) {
+      auto armors = detector.detect(img);
+
+      auto targets = tracker.track(armors, t);
+
+      auto command = aimer.aim(targets, t, cboard.bullet_speed);
+
+      command.shoot = shooter.shoot(command, aimer, targets, ypr);
+
+      cboard.send(command);
+    }
+
+    /// 打符
+    else if (mode == device::Mode::small_buff || mode == device::Mode::big_buff) {
+      buff_solver.set_R_gimbal2world(q);
+
+      auto power_runes = buff_detector.detect(img);
+
+      buff_solver.solve(power_runes);
+
+      device::Command buff_command;
+      if (mode == device::Mode::small_buff) {
+        buff_small_target.get_target(power_runes, t);
+        auto target_copy = buff_small_target;
+        buff_command = buff_aimer.aim(target_copy, t, cboard.bullet_speed, true);
+      } else if (mode == device::Mode::big_buff) {
+        buff_big_target.get_target(power_runes, t);
+        auto target_copy = buff_big_target;
+        buff_command = buff_aimer.aim(target_copy, t, cboard.bullet_speed, true);
+      }
+      cboard.send(buff_command);
+    }
+
+    else
+      continue;
+  }
+
+  return 0;
+}
diff --git a/src/task/balance_infantry_mpc.cpp b/src/task/balance_infantry_mpc.cpp
new file mode 100644
index 0000000..2d0b120
--- /dev/null
+++ b/src/task/balance_infantry_mpc.cpp
@@ -0,0 +1,142 @@
+#include <chrono>
+#include <opencv2/opencv.hpp>
+#include <thread>
+
+#include "src/device/camera.hpp"
+#include "src/device/dm_imu/dm_imu.hpp"
+#include "src/module/auto_aim/aimer.hpp"
+#include "src/module/auto_aim/multithread/commandgener.hpp"
+#include "src/module/auto_aim/multithread/mt_detector.hpp"
+#include "src/module/auto_aim/shooter.hpp"
+#include "src/module/auto_aim/solver.hpp"
+#include "src/module/auto_aim/tracker.hpp"
+#include "src/module/auto_buff/buff_aimer.hpp"
+#include "src/module/auto_buff/buff_detector.hpp"
+#include "src/module/auto_buff/buff_solver.hpp"
+#include "src/module/auto_buff/buff_target.hpp"
+#include "src/module/auto_buff/buff_type.hpp"
+#include "src/component/exiter.hpp"
+#include "src/component/img_tools.hpp"
+#include "src/component/logger.hpp"
+#include "src/component/math_tools.hpp"
+#include "src/component/plotter.hpp"
+#include "src/component/recorder.hpp"
+
+const std::string keys =
+  "{help h usage ? |      | 输出命令行参数说明}"
+  "{@config-path   | configs/standard4.yaml | yaml配置文件路径 }";
+
+using namespace std::chrono_literals;
+
+int main(int argc, char * argv[])
+{
+  cv::CommandLineParser cli(argc, argv, keys);
+  auto config_path = cli.get<std::string>("@config-path");
+  if (cli.has("help") || !cli.has("@config-path")) {
+    cli.printMessage();
+    return 0;
+  }
+
+  component::Exiter exiter;
+  component::Plotter plotter;
+  component::Recorder recorder;
+
+  device::Gimbal gimbal(config_path);
+  device::Camera camera(config_path);
+
+  auto_aim::YOLO yolo(config_path, true);
+  auto_aim::Solver solver(config_path);
+  auto_aim::Tracker tracker(config_path, solver);
+  auto_aim::Planner planner(config_path);
+
+  component::ThreadSafeQueue<std::optional<auto_aim::Target>, true> target_queue(1);
+  target_queue.push(std::nullopt);
+
+  auto_buff::Buff_Detector buff_detector(config_path);
+  auto_buff::Solver buff_solver(config_path);
+  auto_buff::SmallTarget buff_small_target;
+  auto_buff::BigTarget buff_big_target;
+  auto_buff::Aimer buff_aimer(config_path);
+
+  cv::Mat img;
+  Eigen::Quaterniond q;
+  std::chrono::steady_clock::time_point t;
+
+  std::atomic<bool> quit = false;
+
+  std::atomic<device::GimbalMode> mode{device::GimbalMode::IDLE};
+  auto last_mode{device::GimbalMode::IDLE};
+
+  auto plan_thread = std::thread([&]() {
+    while (!quit) {
+      if (!target_queue.empty() && mode == device::GimbalMode::AUTO_AIM) {
+        auto target = target_queue.front();
+        auto gs = gimbal.state();
+        auto plan = planner.plan(target, gs.bullet_speed);
+
+        gimbal.send(
+          plan.control, plan.fire, plan.yaw, plan.yaw_vel, plan.yaw_acc, plan.pitch, plan.pitch_vel,
+          plan.pitch_acc);
+
+        std::this_thread::sleep_for(10ms);
+      } else
+        std::this_thread::sleep_for(200ms);
+    }
+  });
+
+  while (!exiter.exit()) {
+    mode = gimbal.mode();
+
+    if (last_mode != mode) {
+      component::logger()->info("Switch to {}", gimbal.str(mode));
+      last_mode = mode.load();
+    }
+
+    camera.read(img, t);
+    auto q = gimbal.q(t);
+    auto gs = gimbal.state();
+    recorder.record(img, q, t);
+    solver.set_R_gimbal2world(q);
+
+    /// 自瞄
+    if (mode.load() == device::GimbalMode::AUTO_AIM) {
+      auto armors = yolo.detect(img);
+      auto targets = tracker.track(armors, t);
+      if (!targets.empty())
+        target_queue.push(targets.front());
+      else
+        target_queue.push(std::nullopt);
+    }
+
+    /// 打符
+    else if (mode.load() == device::GimbalMode::SMALL_BUFF || mode.load() == device::GimbalMode::BIG_BUFF) {
+      buff_solver.set_R_gimbal2world(q);
+
+      auto power_runes = buff_detector.detect(img);
+
+      buff_solver.solve(power_runes);
+
+      auto_aim::Plan buff_plan;
+      if (mode.load() == device::GimbalMode::SMALL_BUFF) {
+        buff_small_target.get_target(power_runes, t);
+        auto target_copy = buff_small_target;
+        buff_plan = buff_aimer.mpc_aim(target_copy, t, gs, true);
+      } else if (mode.load() == device::GimbalMode::BIG_BUFF) {
+        buff_big_target.get_target(power_runes, t);
+        auto target_copy = buff_big_target;
+        buff_plan = buff_aimer.mpc_aim(target_copy, t, gs, true);
+      }
+      gimbal.send(
+        buff_plan.control, buff_plan.fire, buff_plan.yaw, buff_plan.yaw_vel, buff_plan.yaw_acc,
+        buff_plan.pitch, buff_plan.pitch_vel, buff_plan.pitch_acc);
+
+    } else
+      gimbal.send(false, false, 0, 0, 0, 0, 0, 0);
+  }
+
+  quit = true;
+  if (plan_thread.joinable()) plan_thread.join();
+  gimbal.send(false, false, 0, 0, 0, 0, 0, 0);
+
+  return 0;
+}