重构后的2026赛季哨兵代码
本赛季哨兵为双板结构，小云台和大云台各有一块C板。本代码相应产生了两个分支，原本分支名仅为Gimbal与Chassis，但是由于大量的进度赶工与测试开发并行，此结构已经崩溃，具体请参考以下分支：
Gimbal_IMU_dev分支为小云台实际代码，Chassis_powerlimit分支为目前最新版本，可以视为底盘实际代码。
main分支仅提供基础模板。此外，SMC_test等分支是基于以上两个主分支的测试开发分支，用于滑膜算法等的测试。
代码均以功能为主要布局逻辑，上云台由云台，发射机构，IMU，command四个任务组成。底盘由底盘，IMU，command三个任务组成。其中，command负责整体状态机的控制以及通信的处理。每个任务下有其细分的任务状态机，但总体听从Command的指令。该架构虽然不能和封装到Robot层的架构一样严格决定初始化顺序，但能最大程度减少重复代码，使得状态机更清晰明了。
云台分支使用AHRSMahony解算姿态。底盘分支使用EKF解算姿态。
除任务差异外均使用Fusion
1.上下板间疑似存在通信丢包
2.下板可能存在IMU的异常，偶尔导致疯车
3.底盘跟随PID仍然需要优化
4.小云台大云台相对坐标，转动的逻辑需要更改
1.小云台重力补偿从编码器角度值映射到IMU角度值
2.大云台针对IMU控制的Pid优化
3.功率控制
4.Mahony版本的零漂修正以及先温控再控制零漂的逻辑
5.小云台IMU禁区
6.想起来再说
下板的哨兵代码重构已经在仓库Sentry_Clean_Down进行，目前测试下来，稳定性和丢包现象都有了很大的改善，代码架构上也有优化，尽可能减少复杂度并且单纯地只为上位机控制服务。但是本赛季的情况已经不允许进一步的开发，为了转向步兵也便于维护的代码结构，现已转为开发上位机控制的中间件