简化了电机测量的命名,完成初版平衡底盘的功能编写,待测试方向和符号的正确性

2026-07-24 03:27:45 +08:00 · 2023-04-14 22:26:33 +08:00
parent 9e364cbaaa
commit 1818edf117
14 changed files with 215 additions and 162 deletions
--- a/application/balance_chassis/balance.c
+++ b/application/balance_chassis/balance.c
@@ -26,12 +26,8 @@ static attitude_t *imu_data;
 static SuperCapInstance *super_cap;
 static referee_info_t *referee_data; // 裁判系统的数据会被不断更新
 // 电机
-static HTMotorInstance *lf;
-static HTMotorInstance *rf;
-static HTMotorInstance *lb;
-static HTMotorInstance *rb;
-static LKMotorInstance *l_driven;
-static LKMotorInstance *r_driven;
+static HTMotorInstance *lf, *rf, *lb, *rb;
+static LKMotorInstance *l_driven, *r_driven;
 // 底盘板和云台板通信
 static CANCommInstance *chassis_comm; // 由于采用多板架构,即使使用C板也有空余串口,可以使用串口通信以获得更高的通信速率并降低阻塞
 static Chassis_Ctrl_Cmd_s chassis_cmd_recv;
@@ -40,27 +36,51 @@ static Chassis_Upload_Data_s chassis_feed_send;
 static LinkNPodParam left_side, right_side;
 static PIDInstance swerving_pid;        // 转向PID,有转向指令时使用IMU的加速度反馈积分以获取速度和位置状态量
 static PIDInstance anti_crash_pid;      // 抗劈叉,将输出以相反的方向叠加到左右腿的上
-static PIDInstance leg_length_pid;      // 用PD模拟弹簧的传递函数,不需要积分项(弹簧是一个无积分的二阶系统),增益不可过大否则抗外界冲击响应时太"硬"
+static PIDInstance leg_length_pid;      // 用PD模拟弹簧,不要积分(弹簧是无积分二阶系统),增益不可过大否则抗外界冲击响应时太"硬"
 static PIDInstance roll_compensate_pid; // roll轴补偿,用于保持机体水平

 /* ↓↓↓分割出这些函数是为了提高可读性,使得阅读者的逻辑更加顺畅;但实际上这些函数都不长,可以以注释的形式直接加到BalanceTask里↓↓↓*/

 /**
 * @brief 将电机和imu的数据组装为LinkNPodParam结构体
+ * @note 由于两个连杆和轮子连接时,只有一个使用了轴承,另外一个直接和电机的定子连接,
+ *       所以轮子的转速和电机的转速不一致,因此和地面的速度应为电机转速减去杆的转速,得到的才是轮子的转速
 *
+ * @todo angle direction to be verified
 */
 static void ParamAssemble()
-{
+{ // 电机的角度是逆时针为正,左侧全部取反
+    left_side.phi1 = PI2 - LIMIT_LINK_RAD - lb->measure.total_angle;
+    left_side.phi4 = -lf->measure.total_angle + LIMIT_LINK_RAD;
+    left_side.phi1_w = -lb->measure.speed_rads;
+    left_side.phi4_w = -lf->measure.speed_rads;
+    left_side.wheel_dist = -l_driven->measure.total_angle / 360 * WHEEL_RADIUS * PI;
+    left_side.wheel_w = -l_driven->measure.speed_rads;
+
+    right_side.phi1 = PI2 - LIMIT_LINK_RAD + rb->measure.total_angle;
+    right_side.phi4 = rf->measure.total_angle + LIMIT_LINK_RAD;
+    right_side.phi1_w = rb->measure.speed_rads;
+    right_side.phi4_w = rf->measure.speed_rads;
+    right_side.wheel_dist = r_driven->measure.total_angle / 360 * WHEEL_RADIUS * PI;
+    right_side.wheel_w = r_driven->measure.speed_rads;
+
+    if(chassis_cmd_recv.wz != 0) // 若有转向指令,则使用IMU积分得到的位置,否则左右轮位置相反会产生阻碍转向的力矩
+    {
+        // left_side.wheel_dist = 0.5*imu_data->Accel[3];
+        // right_side.wheel_dist = 0.5*imu_data->Accel[3];
+    }
 }

 /**
 * @brief 根据关节角度和角速度,计算单杆长度和角度以及变化率
+ * @todo 测试两种的效果,留下其中一种;
+ *       若差分的效果好则不需要VELOCITY_DIFF_VMC内的代码
 *
 * @param p 5连杆和腿的参数
 */
 static void Link2Pod(LinkNPodParam *p)
 { // 拟将功能封装到vmc_project.h中
-    float xD, yD, xB, yB, BD, A0, B0, xC, yC, phi2;
+    float xD, yD, xB, yB, BD, A0, B0, xC, yC, phi2t, phi5t;
    xD = JOINT_DISTANCE + THIGH_LEN * arm_cos_f32(p->phi4);
    yD = THIGH_LEN * arm_sin_f32(p->phi4);
    xB = 0 + THIGH_LEN * arm_cos_f32(p->phi1);
@@ -68,20 +88,16 @@ static void Link2Pod(LinkNPodParam *p)
    BD = powf(xD - xB, 2) + powf(yD - yB, 2);
    A0 = 2 * CALF_LEN * (xD - xB);
    B0 = 2 * CALF_LEN * (yD - yB);
-    p->phi2 = 2 * atan2f(B0 + Sqrt(powf(A0, 2) + powf(B0, 2) - powf(BD, 2)), A0 + BD);
-    xC = xB + CALF_LEN * arm_cos_f32(p->phi2);
-    yC = yB + CALF_LEN * arm_sin_f32(p->phi2);
-#ifdef ANGLE_DIFF_VMC
-    float p5t = atan2f(yC, xC - JOINT_DISTANCE / 2); // 避免重复计算
-    float plt = Sqrt(powf(xC - JOINT_DISTANCE / 2, 2) + powf(yC, 2));
-    p->phi5_w = (p5t - p->phi5) / balance_dt;
-    p->pod_len_w = (plt - p->pod_len) / balance_dt;
-    p->phi5 = p5t;
-    p->pod_len = plt;
-#endif
+    phi2t = 2 * atan2f(B0 + Sqrt(powf(A0, 2) + powf(B0, 2) - powf(BD, 2)), A0 + BD);
+    xC = xB + CALF_LEN * arm_cos_f32(phi2t);
+    yC = yB + CALF_LEN * arm_sin_f32(phi2t);
+
+    p->phi2 = phi2t;
    p->phi5 = atan2f(yC, xC - JOINT_DISTANCE / 2);
    p->pod_len = Sqrt(powf(xC - JOINT_DISTANCE / 2, 2) + powf(yC, 2));
    p->phi3 = atan2(yC - yD, xC - xD); // 稍后用于计算VMC
+    p->theta = p->phi5 - 0.5 * PI - imu_data->Pitch;
+    p->height = p->pod_len * arm_cos_f32(p->theta);

 #ifdef VELOCITY_DIFF_VMC
    float phi1_pred = p->phi1 + p->phi1_w * balance_dt; // 预测下一时刻的关节角度(利用关节角速度)
@@ -94,24 +110,46 @@ static void Link2Pod(LinkNPodParam *p)
    BD = powf(xD - xB, 2) + powf(yD - yB, 2);
    A0 = 2 * CALF_LEN * (xD - xB);
    B0 = 2 * CALF_LEN * (yD - yB);
-    phi2 = 2 * atan2f(B0 + Sqrt(powf(A0, 2) + powf(B0, 2) - powf(BD, 2)), A0 + BD); // 不要用link->phi2,因为这里是预测的
-    xC = xB + CALF_LEN * arm_cos_f32(phi2);
-    yC = yB + CALF_LEN * arm_sin_f32(phi2);
+    phi2t = 2 * atan2f(B0 + Sqrt(powf(A0, 2) + powf(B0, 2) - powf(BD, 2)), A0 + BD); // 不要用link->phi2,因为这里是预测的
+    xC = xB + CALF_LEN * arm_cos_f32(phi2t);
+    yC = yB + CALF_LEN * arm_sin_f32(phi2t);
+    phi5t = atan2f(yC, xC - JOINT_DISTANCE / 2);
    // 差分计算腿长变化率和腿角速度
-    p->pod_w = (atan2f(yC, xC - JOINT_DISTANCE / 2) - p->phi5) / balance_dt;
+    p->phi2_w = (phi2t - p->phi2) / balance_dt; // 稍后用于修正轮速
+    p->pod_w = (phi5t - p->phi5) / balance_dt;
    p->pod_v = (Sqrt(powf(xC - JOINT_DISTANCE / 2, 2) + powf(yC, 2)) - p->pod_len) / balance_dt;
-#endif // VELOCITY_DIFF_VMC
+    p->theta_w = (phi5t - 0.5 * PI - imu_data->Pitch - p->theta) / balance_dt;
+    p->height_v = p->pod_v * arm_cos_f32(p->theta) - p->pod_len * arm_sin_f32(p->theta) * p->theta_w; // 这很酷!PDE!
+#endif
+
+    p->wheel_w = (p->wheel_w - p->phi2_w + imu_data->Gyro[3]) * WHEEL_RADIUS; // 修正轮速
 }

 /**
 * @brief 根据状态反馈计算当前腿长,查表获得LQR的反馈增益,并列式计算LQR的输出
 *        由于反馈矩阵和控制矩阵都比较稀疏,故不使用矩阵库,避免非零项计算
+ *
 * @note 计算得到的腿部力矩输出还要再综合运动控制系统补偿后映射为两个关节电机
 *       而轮子的输出则只经过转向PID的反馈增益计算
 *
+ * @todo 确定使用查表还是多项式拟合
+ *
 */
-static void CalcLQR(LinkNPodParam *p)
+static void CalcLQR(LinkNPodParam *p, float target_x)
 {
+    float *gain_list = LookUpKgain(p->pod_len); // K11,K12... K21,K22... K26
+    float T[2];                                 // 0 T_wheel, 1 T_pod;
+    for (uint8_t i = 0; i < 2; i++)
+    {
+        T[i] = gain_list[i * 6 + 0] * -p->theta +
+               gain_list[i * 6 + 1] * -p->theta_w +
+               gain_list[i * 6 + 2] * (target_x - p->wheel_dist) +
+               gain_list[i * 6 + 3] * -p->wheel_w +
+               gain_list[i * 6 + 4] * -imu_data->Pitch +
+               gain_list[i * 6 + 5] * -imu_data->Gyro[3];
+    }
+    p->T_wheel = T[0];
+    p->T_pod = T[1];
 }

 /**
@@ -124,7 +162,7 @@ static void SynthesizeMotion()
    left_side.T_pod += anti_crash_pid.Output;
    right_side.T_pod -= anti_crash_pid.Output;

-    // PIDCalculate(&swerving_pid, imu_data->Yaw, 0); // 对速度闭环还是使用角度增量闭环?
+    PIDCalculate(&swerving_pid, imu_data->Yaw, chassis_cmd_recv.wz); // 对速度闭环还是使用角度增量闭环?
    left_side.T_wheel -= swerving_pid.Output;
    right_side.T_wheel += swerving_pid.Output;
 }
@@ -135,7 +173,7 @@ static void SynthesizeMotion()
 */
 static void LegControl(LinkNPodParam *p, float target_length)
 {
-    p->F_pod += PIDCalculate(&leg_length_pid, p->pod_len, target_length);
+    p->F_pod += PIDCalculate(&leg_length_pid, p->height, target_length);
 }

 /**
@@ -177,8 +215,14 @@ static void FlyDetect()
 * @brief 功率限制,一般不需要
 *
 */
-static void WattLimit(LinkNPodParam *p)
+static void WattLimit()
 {
+    HTMotorSetRef(&lf, left_side.T_front);
+    HTMotorSetRef(&lb, left_side.T_back);
+    HTMotorSetRef(&rf, right_side.T_front);
+    HTMotorSetRef(&rb, right_side.T_back);
+    LKMotorSetRef(&l_driven, left_side.T_wheel);
+    LKMotorSetRef(&r_driven, right_side.T_wheel);
 }

 void BalanceInit()
@@ -320,13 +364,32 @@ void BalanceTask()
 {
    chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(chassis_comm);

+    if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
+    {
+        HTMotorStop(lf);
+        HTMotorStop(rf);
+        HTMotorStop(lb);
+        HTMotorStop(rb);
+        LKMotorStop(l_driven);
+        LKMotorStop(r_driven);
+    }
+    else
+    {
+        HTMotorEnable(lf);
+        HTMotorEnable(rf);
+        HTMotorEnable(lb);
+        HTMotorEnable(rb);
+        LKMotorEnable(l_driven);
+        LKMotorEnable(r_driven);
+    }
+
    ParamAssemble(); // 参数组装,将电机和IMU的参数组装到一起
    // 将五连杆映射成单杆
    Link2Pod(&left_side);
    Link2Pod(&right_side);
    // 根据单杆计算处的角度和杆长,计算反馈增益
-    CalcLQR(&left_side);
-    CalcLQR(&right_side);
+    CalcLQR(&left_side, chassis_cmd_recv.vx); // @todo,需要确定速度or位置闭环
+    CalcLQR(&right_side, chassis_cmd_recv.vx);
    // 腿长控制
    LegControl(&left_side, 0);
    LegControl(&right_side, 0);
@@ -339,11 +402,10 @@ void BalanceTask()
    VMCProject(&right_side);

    FlyDetect(); // 滞空检测
-    // 电机输出限幅
-    WattLimit(&left_side);
-    WattLimit(&right_side);
+
+    WattLimit(); // 电机输出限幅

    // code to go here... 裁判系统,UI,多机通信
-    
-    CANCommSend(chassis_comm, (uint8_t*)&chassis_feed_send);
+
+    CANCommSend(chassis_comm, (uint8_t *)&chassis_feed_send);
 }
--- a/application/balance_chassis/balance.h
+++ b/application/balance_chassis/balance.h
@@ -4,31 +4,30 @@
 #define CALF_LEN 0.25f        // 小腿
 #define THIGH_LEN 0.15f       // 大腿
 #define JOINT_DISTANCE 0.108f // 关节间距
+#define WHEEL_RADIUS 0.032f   // 轮子半径
+#define LIMIT_LINK_RAD 0.358  //

-#define VELOCITY_DIFF_VMC //通过速度计算增量,然后通过差分计算腿长变化率和腿角速度
+// 计算速度的方式(五连杆到单杆的映射)
+#define VELOCITY_DIFF_VMC // 通过速度计算增量,然后通过差分计算腿长变化率和腿角速度
 // #define ANGLE_DIFF_VMC //直接保存上一次的值,通过差分计算腿长变化率和腿角速度

-
 typedef struct
 {
    // joint
-    float phi1_w, phi4_w;
+    float phi1_w, phi4_w, phi2_w; // phi2_w used for calc real wheel speed
    float T_back, T_front;
-
    // link angle,phi1-ph5, phi5 is pod angle
    float phi1, phi2, phi3, phi4, phi5;
-
    // wheel
-    float wheel_angle;
+    float wheel_dist;
    float wheel_w;
    float T_wheel;
-
    // pod
+    float theta, theta_w; // 杆和垂直方向的夹角,为控制状态之一
    float pod_len;
-    float pod_w;
-    float pod_v;
-    float F_pod;
-    float T_pod;
+    float height,height_v;
+    float pod_v, pod_w;
+    float F_pod, T_pod;
 } LinkNPodParam;

 /**
--- a/application/balance_chassis/gain_table.h
+++ b/application/balance_chassis/gain_table.h
@@ -19,7 +19,7 @@ void EnalbeInterpolation(void)
 }

 /* 默认关闭插值,向下取整 */
-float LookUpKgain(float leg_length)
+float* LookUpKgain(float leg_length)
 {
-
+    
 }
--- a/application/chassis/chassis.c
+++ b/application/chassis/chassis.c
@@ -43,18 +43,14 @@ attitude_t *Chassis_IMU_data;
 #ifdef ONE_BOARD
 static Publisher_t *chassis_pub;                    // 用于发布底盘的数据
 static Subscriber_t *chassis_sub;                   // 用于订阅底盘的控制命令
-#endif // !ONE_BOARD
+#endif                                              // !ONE_BOARD
 static Chassis_Ctrl_Cmd_s chassis_cmd_recv;         // 底盘接收到的控制命令
 static Chassis_Upload_Data_s chassis_feedback_data; // 底盘回传的反馈数据

-// static referee_info_t *referee_data; // 裁判系统相关数据
-static SuperCapInstance *cap;        // 超级电容
-static DJIMotorInstance *motor_lf;   // left right forward back
-static DJIMotorInstance *motor_rf;
-static DJIMotorInstance *motor_lb;
-static DJIMotorInstance *motor_rb;
+static SuperCapInstance *cap;                                       // 超级电容
+static DJIMotorInstance *motor_lf, *motor_rf, *motor_lb, *motor_rb; // left right forward back

-/* 用于自旋变速策略的时间变量,后续考虑查表加速 */
+/* 用于自旋变速策略的时间变量 */
 // static float t;

 /* 私有函数计算的中介变量,设为静态避免参数传递的开销 */
@@ -68,19 +64,19 @@ void ChassisInit()
        .can_init_config.can_handle = &hcan1,
        .controller_param_init_config = {
            .speed_PID = {
-                .Kp = 10,//4.5
-                .Ki = 0,//0
-                .Kd = 0,//0
+                .Kp = 10, // 4.5
+                .Ki = 0,  // 0
+                .Kd = 0,  // 0
                .IntegralLimit = 3000,
-                 .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement,
+                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                .MaxOut = 12000,
            },
            .current_PID = {
-                .Kp = 0.5,//0.4
-                .Ki = 0,//0
+                .Kp = 0.5, // 0.4
+                .Ki = 0,   // 0
                .Kd = 0,
                .IntegralLimit = 3000,
-                 .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement,
+                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                .MaxOut = 15000,
            },
        },
@@ -114,7 +110,6 @@ void ChassisInit()
    // while (referee_data->GameRobotState.robot_id ==0);
    // Referee_Interactive_init(referee_data);

-
    SuperCap_Init_Config_s cap_conf = {
        .can_config = {
            .can_handle = &hcan2,
@@ -145,10 +140,10 @@ void ChassisInit()
 #endif // ONE_BOARD
 }

-#define LF_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * ANGLE_2_RAD)
-#define RF_CENTER ((HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * ANGLE_2_RAD)
-#define LB_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y) * ANGLE_2_RAD)
-#define RB_CENTER ((HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y) * ANGLE_2_RAD)
+#define LF_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
+#define RF_CENTER ((HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
+#define LB_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
+#define RB_CENTER ((HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
 /**
 * @brief 计算每个轮毂电机的输出,正运动学解算
 *        用宏进行预替换减小开销,运动解算具体过程参考教程
@@ -157,7 +152,7 @@ static void MecanumCalculate()
 {
    vt_lf = -chassis_vx - chassis_vy - chassis_cmd_recv.wz * LF_CENTER;
    vt_rf = -chassis_vx + chassis_vy - chassis_cmd_recv.wz * RF_CENTER;
-    vt_lb = chassis_vx - chassis_vy -chassis_cmd_recv.wz * LB_CENTER;
+    vt_lb = chassis_vx - chassis_vy - chassis_cmd_recv.wz * LB_CENTER;
    vt_rb = chassis_vx + chassis_vy - chassis_cmd_recv.wz * RB_CENTER;
 }

@@ -224,10 +219,10 @@ void ChassisTask()
        chassis_cmd_recv.wz = 0;
        break;
    case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出
-        chassis_cmd_recv.wz = -1.5*chassis_cmd_recv.offset_angle*abs(chassis_cmd_recv.offset_angle);
+        chassis_cmd_recv.wz = -1.5 * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle);
        break;
    case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
-        chassis_cmd_recv.wz=4000;
+        chassis_cmd_recv.wz = 4000;
        break;
    default:
        break;
@@ -236,8 +231,8 @@ void ChassisTask()
    // 根据云台和底盘的角度offset将控制量映射到底盘坐标系上
    // 底盘逆时针旋转为角度正方向;云台命令的方向以云台指向的方向为x,采用右手系(x指向正北时y在正东)
    static float sin_theta, cos_theta;
-    cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * ANGLE_2_RAD);
-    sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * ANGLE_2_RAD);
+    cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
+    sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
    chassis_vx = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta;
    chassis_vy = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta;

--- a/application/gimbal/gimbal.c
+++ b/application/gimbal/gimbal.c
@@ -7,16 +7,15 @@

 #include "bmi088.h"

-static attitude_t *gimba_IMU_data;   // 云台IMU数据
-static DJIMotorInstance *yaw_motor;   // yaw电机
-static DJIMotorInstance *pitch_motor; // pitch电机
+static attitude_t *gimba_IMU_data; // 云台IMU数据
+static DJIMotorInstance *yaw_motor, *pitch_motor;

 static Publisher_t *gimbal_pub;                   // 云台应用消息发布者(云台反馈给cmd)
 static Subscriber_t *gimbal_sub;                  // cmd控制消息订阅者
 static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息
 static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv;         // 来自cmd的控制信息

-BMI088Instance* imu;
+BMI088Instance *imu;
 void GimbalInit()
 {
    BMI088_Init_Config_s imu_config = {
@@ -54,7 +53,7 @@ void GimbalInit()
        .cali_mode = BMI088_CALIBRATE_ONLINE_MODE,
        .work_mode = BMI088_BLOCK_PERIODIC_MODE,
    };
-    imu=BMI088Register(&imu_config);
+    imu = BMI088Register(&imu_config);
    // gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
    // YAW
    Motor_Init_Config_s yaw_config = {
@@ -64,26 +63,26 @@ void GimbalInit()
        },
        .controller_param_init_config = {
            .angle_PID = {
-                .Kp = 8, //8
+                .Kp = 8, // 8
                .Ki = 0,
                .Kd = 0,
                .DeadBand = 0.1,
-                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement,
+                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                .IntegralLimit = 100,

                .MaxOut = 500,
            },
            .speed_PID = {
-                .Kp = 50,//50
-                .Ki = 200,//200
+                .Kp = 50,  // 50
+                .Ki = 200, // 200
                .Kd = 0,
-                .Improve = PID_Trapezoid_Intergral |PID_Integral_Limit |PID_Derivative_On_Measurement,
+                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                .IntegralLimit = 3000,
                .MaxOut = 20000,
            },
            .other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle,
            // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
-            .other_speed_feedback_ptr=&gimba_IMU_data->Gyro[2],
+            .other_speed_feedback_ptr = &gimba_IMU_data->Gyro[2],
        },
        .controller_setting_init_config = {
            .angle_feedback_source = OTHER_FEED,
@@ -109,11 +108,11 @@ void GimbalInit()
                .MaxOut = 500,
            },
            .speed_PID = {
-                .Kp=50,//50
-                .Ki =350,//350
-                .Kd =0,//0
-                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |PID_Derivative_On_Measurement,
-                .IntegralLimit =2500,
+                .Kp = 50,  // 50
+                .Ki = 350, // 350
+                .Kd = 0,   // 0
+                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
+                .IntegralLimit = 2500,
                .MaxOut = 20000,
            },
            .other_angle_feedback_ptr = &gimba_IMU_data->Pitch,
@@ -185,7 +184,7 @@ void GimbalTask()

    // 设置反馈数据,主要是imu和yaw的ecd
    gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data;
-    gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->motor_measure.angle_single_round;
+    gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->measure.angle_single_round;

    // 推送消息
    PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);
--- a/application/shoot/shoot.c
+++ b/application/shoot/shoot.c
@@ -7,9 +7,7 @@
 #include "general_def.h"

 /* 对于双发射机构的机器人,将下面的数据封装成结构体即可,生成两份shoot应用实例 */
-static DJIMotorInstance *friction_l; // 左摩擦轮
-static DJIMotorInstance *friction_r; // 右摩擦轮
-static DJIMotorInstance *loader;     // 拨盘电机
+static DJIMotorInstance *friction_l, *friction_r, *loader; // 拨盘电机
 // static servo_instance *lid; 需要增加弹舱盖

 static Publisher_t *shoot_pub;
@@ -29,16 +27,16 @@ void ShootInit()
        },
        .controller_param_init_config = {
            .speed_PID = {
-                .Kp = 0,//20
-                .Ki = 0,//1
+                .Kp = 0, // 20
+                .Ki = 0, // 1
                .Kd = 0,
                .Improve = PID_Integral_Limit,
                .IntegralLimit = 10000,
                .MaxOut = 15000,
            },
            .current_PID = {
-                .Kp = 0,//0.7
-                .Ki = 0,//0.1
+                .Kp = 0, // 0.7
+                .Ki = 0, // 0.1
                .Kd = 0,
                .Improve = PID_Integral_Limit,
                .IntegralLimit = 10000,
@@ -54,13 +52,13 @@ void ShootInit()
            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
        },
        .motor_type = M3508};
-        friction_config.can_init_config.tx_id = 1,
-        friction_l = DJIMotorInit(&friction_config);
-        
-        friction_config.can_init_config.tx_id = 2; // 右摩擦轮,改txid和方向就行
-        friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
-        friction_r = DJIMotorInit(&friction_config); 
-    
+    friction_config.can_init_config.tx_id = 1,
+    friction_l = DJIMotorInit(&friction_config);
+
+    friction_config.can_init_config.tx_id = 2; // 右摩擦轮,改txid和方向就行
+    friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
+    friction_r = DJIMotorInit(&friction_config);
+
    // 拨盘电机
    Motor_Init_Config_s loader_config = {
        .can_init_config = {
@@ -70,22 +68,22 @@ void ShootInit()
        .controller_param_init_config = {
            .angle_PID = {
                // 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降
-                .Kp = 0,//10
+                .Kp = 0, // 10
                .Ki = 0,
                .Kd = 0,
                .MaxOut = 200,
            },
            .speed_PID = {
-                .Kp = 0,//10
-                .Ki = 0,//1
+                .Kp = 0, // 10
+                .Ki = 0, // 1
                .Kd = 0,
                .Improve = PID_Integral_Limit,
                .IntegralLimit = 5000,
                .MaxOut = 5000,
            },
            .current_PID = {
-                .Kp = 0,//0.7
-                .Ki = 0,//0.1
+                .Kp = 0, // 0.7
+                .Ki = 0, // 0.1
                .Kd = 0,
                .Improve = PID_Integral_Limit,
                .IntegralLimit = 5000,
@@ -140,16 +138,16 @@ void ShootTask()
        DJIMotorSetRef(loader, 0);             // 同时设定参考值为0,这样停止的速度最快
        break;
    // 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入,导致多次发射)
-    case LOAD_1_BULLET: // 激活能量机关/干扰对方用,英雄用.
-        DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到角度环
-        DJIMotorSetRef(loader, loader->motor_measure.total_angle + ONE_BULLET_DELTA_ANGLE); // 控制量增加一发弹丸的角度
+    case LOAD_1_BULLET:                                                                     // 激活能量机关/干扰对方用,英雄用.
+        DJIMotorOuterLoop(loader, ANGLE_LOOP);                                              // 切换到角度环
+        DJIMotorSetRef(loader, loader->measure.total_angle + ONE_BULLET_DELTA_ANGLE); // 控制量增加一发弹丸的角度
        hibernate_time = DWT_GetTimeline_ms();                                              // 记录触发指令的时间
        dead_time = 150;                                                                    // 完成1发弹丸发射的时间
        break;
    // 三连发,如果不需要后续可能删除
    case LOAD_3_BULLET:
-        DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到速度环
-        DJIMotorSetRef(loader, loader->motor_measure.total_angle + 3 * ONE_BULLET_DELTA_ANGLE); // 增加3发
+        DJIMotorOuterLoop(loader, ANGLE_LOOP);                                                  // 切换到速度环
+        DJIMotorSetRef(loader, loader->measure.total_angle + 3 * ONE_BULLET_DELTA_ANGLE); // 增加3发
        hibernate_time = DWT_GetTimeline_ms();                                                  // 记录触发指令的时间
        dead_time = 300;                                                                        // 完成3发弹丸发射的时间
        break;
@@ -170,7 +168,6 @@ void ShootTask()
            ; // 未知模式,停止运行,检查指针越界,内存溢出等问题
    }

-    
    // 确定是否开启摩擦轮,后续可能修改为键鼠模式下始终开启摩擦轮(上场时建议一直开启)
    if (shoot_cmd_recv.friction_mode == FRICTION_ON)
    {
@@ -193,7 +190,7 @@ void ShootTask()
            DJIMotorSetRef(friction_l, 30000);
            DJIMotorSetRef(friction_r, 30000);
            break;
-        } 
+        }
    }
    else // 关闭摩擦轮
    {