TuxMonkey/bf_original_balance_chassis
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-07-24 03:27:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

我草可以了，但是有点不平

Browse Source
This commit is contained in:
TuxMonkey
2026-07-15 14:29:29 +08:00
parent 709e66dcd4
commit 12be1e96de
1 changed files with 5 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
application/chassis/balance.c
View File

@@ -31,7 +31,8 @@
#define STOOL_PITCH_K 24.0f #define STOOL_PITCH_K 24.0f
#define STOOL_PITCH_W_K 6.8f #define STOOL_PITCH_W_K 6.8f
#define STOOL_VEL_K 3.2f #define STOOL_VEL_K 3.2f
#define STOOL_STOP_DIST_K 1.2f #define STOOL_STOP_DIST_K 0.0f
#define STOOL_PITCH_REF -0.05f
#define STOOL_VEL_LPF_RC 0.03f #define STOOL_VEL_LPF_RC 0.03f
#define STOOL_WHEEL_TORQUE_LIMIT 25.0f #define STOOL_WHEEL_TORQUE_LIMIT 25.0f
#define STOOL_RC_DEADBAND 40 #define STOOL_RC_DEADBAND 40
@@ -430,7 +431,7 @@ static void ParamAssemble()
{ {
// 机体参数,视为平面刚体 // 机体参数,视为平面刚体
chassis.pitch = Chassis_IMU_data->Pitch * DEGREE_2_RAD; chassis.pitch = Chassis_IMU_data->Pitch * DEGREE_2_RAD;
chassis.pitch_w = Chassis_IMU_data->Gyro[0]; chassis.pitch_w = Chassis_IMU_data->Gyro[1];
chassis.yaw = Chassis_IMU_data->YawTotalAngle * DEGREE_2_RAD; chassis.yaw = Chassis_IMU_data->YawTotalAngle * DEGREE_2_RAD;
chassis.wz = Chassis_IMU_data->Gyro[2]; chassis.wz = Chassis_IMU_data->Gyro[2];
chassis.roll = Chassis_IMU_data->Roll * DEGREE_2_RAD; chassis.roll = Chassis_IMU_data->Roll * DEGREE_2_RAD;
@@ -463,7 +464,8 @@ static void StoolModeBalanceControl(void)
stool_stop_dist = stool_dist; stool_stop_dist = stool_dist;
float stop_dist_error = stool_dist - stool_stop_dist; float stop_dist_error = stool_dist - stool_stop_dist;
float wheel_torque = -STOOL_PITCH_K * chassis.pitch - float pitch_error = chassis.pitch - STOOL_PITCH_REF;
float wheel_torque = -STOOL_PITCH_K * pitch_error -
STOOL_PITCH_W_K * chassis.pitch_w + STOOL_PITCH_W_K * chassis.pitch_w +
STOOL_VEL_K * (stool_vel - chassis.target_v) + STOOL_VEL_K * (stool_vel - chassis.target_v) +
STOOL_STOP_DIST_K * stop_dist_error; STOOL_STOP_DIST_K * stop_dist_error;