小陀螺时移动速度置0

application/chassis/balance.c

@@ -20,6 +20,7 @@
 #include "speed_estimation.h"
 #include "fly_detection.h"
 #include "buzzer.h"
+
 // 计时变量
 static uint32_t balance_dwt_cnt;
 static float del_t;
@@ -29,6 +30,7 @@ static INS_t *Chassis_IMU_data;
 static RC_ctrl_t *rc_data; // 底盘单独调试用
 static Chassis_Ctrl_Cmd_s chassis_cmd_recv;
 static Chassis_Upload_Data_s chassis_feedback_data; // 底盘反馈数据
+
 // 四个关节电机和两个驱动轮电机
 static HTMotorInstance *lf, *lb, *rf, *rb, *joint[4]; // 指针数组方便传参和调试
 static LKMotorInstance *l_driven, *r_driven, *driven[2];
@@ -68,6 +70,7 @@ void BalanceInit()
         .send_data_len = sizeof(Chassis_Upload_Data_s),
     };
     cmd_can_comm = CANCommInit(&comm_conf);
+
     // 关节电机
     Motor_Init_Config_s joint_conf = {
         // 写一个,剩下的修改方向和id即可
@@ -256,7 +259,6 @@ static void ControlSwitch()
     //     chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(cmd_can_comm);
     // }
     chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(cmd_can_comm);
-
 }
 
 /* 腿缩回复位,只允许驱动轮电机移动 */
@@ -353,7 +355,8 @@ static void WokingStateSet()
     {
         chassis.target_yaw = chassis_cmd_recv.offset_angle;
     }
-    chassis.target_yaw = chassis.yaw + chassis_cmd_recv.offset_angle*DEGREE_2_RAD;
+    chassis.target_yaw = chassis.yaw + chassis_cmd_recv.offset_angle * DEGREE_2_RAD;
+
     // TODO 转向速度限幅
 
     // TODO 最大dist误差限幅
@@ -395,12 +398,12 @@ static void ParamAssemble()
 static void SynthesizeMotion() /* 腿部控制:抗劈叉; 轮子控制:转向 */
 {
     if(chassis_cmd_recv.chassis_mode == CHASSIS_FREE_DEBUG || 
-        chassis_cmd_recv.chassis_mode == CHASSIS_FOLLOW_GIMBAL_YAW)
+        chassis_cmd_recv.chassis_mode == CHASSIS_FOLLOW_GIMBAL_YAW) // 底盘跟随
     {
         float p_ref = PIDCalculate(&steer_p_pid, chassis.yaw, chassis.target_yaw);
         PIDCalculate(&steer_v_pid, chassis.wz, p_ref);
     }
-    else if (chassis_cmd_recv.chassis_mode == CHASSIS_ROTATE)
+    else if (chassis_cmd_recv.chassis_mode == CHASSIS_ROTATE)   // 小陀螺
     {
         PIDCalculate(&steer_v_pid, chassis.wz, 6);
     }
@@ -441,6 +444,7 @@ static void WattLimitSet() /* 设定运动模态的输出 */
 void BalanceTask()
 {
     del_t = DWT_GetDeltaT(&balance_dwt_cnt);
+    
     BuzzerOn();
     // 切换遥控器控制or云台板控制
     ControlSwitch();