TuxMonkey/bf_original_balance_chassis
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-07-23 19:25:09 +08:00
Code Issues Packages Projects Releases Wiki Activity

小板凳 存档

Browse Source
This commit is contained in:
TuxMonkey
2026-07-15 19:30:07 +08:00
parent 5693c30faf
commit 14a3ab4cd0
1 changed files with 26 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

36
application/chassis/balance.c
View File

@@ -24,14 +24,16 @@
#include "math.h"
#include "string.h"
#define STOOL_SPEED_REF 1.8f
#define STOOL_YAW_RATE_REF 2.4f
#define STOOL_ACC_REF 6.5f
#define STOOL_BRAKE_ACC_REF 16.0f
#define STOOL_PITCH_K 50.0f
#define STOOL_PITCH_W_K 12.5f
#define STOOL_VEL_K 6.2f
#define STOOL_STOP_DIST_K 3.0f
#define STOOL_SPEED_REF 1.5f
#define STOOL_YAW_RATE_REF 3.5f
#define STOOL_SPIN_YAW_RATE_REF 10.0f
#define STOOL_SPIN_YAW_ACC_REF 5.0f
#define STOOL_ACC_REF 4.0f
#define STOOL_BRAKE_ACC_REF 8.0f
#define STOOL_PITCH_K 55.0f
#define STOOL_PITCH_W_K 7.0f
#define STOOL_VEL_K 10.0f
#define STOOL_STOP_DIST_K 8.0f
#define STOOL_PITCH_REF -0.10f
#define STOOL_VEL_LPF_RC 0.03f
#define STOOL_WHEEL_TORQUE_LIMIT 25.0f
@@ -73,6 +75,7 @@ static float stool_vel;
static float stool_dist;
static float stool_stop_dist;
static float stool_target_wz;
static uint8_t stool_spin_enabled;
static chassis_mode_e last_chassis_mode = CHASSIS_ZERO_FORCE;
static void ClearMotionTargets(void)
@@ -80,6 +83,7 @@ static void ClearMotionTargets(void)
chassis.target_v = 0.0f;
chassis.target_dist = chassis.dist;
stool_target_wz = 0.0f;
stool_spin_enabled = 0;
}
static void ClearControlOutput(void)
@@ -337,6 +341,7 @@ static void ControlSwitch()
chassis_cmd_recv.loader_mode = LOAD_STOP;
chassis_cmd_recv.vision_mode = UNLOCK;
chassis_cmd_recv.ui_mode = UI_KEEP;
stool_spin_enabled = 0;
if (!RemoteControlIsOnline())
{
@@ -357,7 +362,15 @@ static void ControlSwitch()
chassis_cmd_recv.chassis_mode = CHASSIS_STOOL_MODE;
chassis_cmd_recv.vx = RCChannelToRatio(rc_data[TEMP].rc.rocker_r1) * STOOL_SPEED_REF;
chassis_cmd_recv.offset_angle = -RCChannelToRatio(rc_data[TEMP].rc.rocker_r_) * STOOL_YAW_RATE_REF;
if (switch_is_up(switch_right))
{
chassis_cmd_recv.offset_angle = -STOOL_SPIN_YAW_RATE_REF;
stool_spin_enabled = 1;
}
else
{
chassis_cmd_recv.offset_angle = -RCChannelToRatio(rc_data[TEMP].rc.rocker_r_) * STOOL_YAW_RATE_REF;
}
}
/* Joint motors must remain disabled on this demo chassis. */
@@ -402,7 +415,10 @@ static void WokingStateSet()
EnableDrivenMotor();
chassis_status = ROBOT_READY;
l_side.target_len = r_side.target_len = 0.12f;
stool_target_wz = chassis_cmd_recv.offset_angle;
if (stool_spin_enabled || fabsf(stool_target_wz) > STOOL_YAW_RATE_REF)
ApproachFloat(&stool_target_wz, chassis_cmd_recv.offset_angle, STOOL_SPIN_YAW_ACC_REF * del_t);
else
stool_target_wz = chassis_cmd_recv.offset_angle;
if (fabsf(chassis_cmd_recv.vx) > 0.001f)
{