TuxMonkey/bf_original_balance_chassis
1
1
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-07-24 19:47:44 +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 "math.h"
#include "string.h" #include "string.h"
#define STOOL_SPEED_REF 1.8f #define STOOL_SPEED_REF 1.5f
#define STOOL_YAW_RATE_REF 2.4f #define STOOL_YAW_RATE_REF 3.5f
#define STOOL_ACC_REF 6.5f #define STOOL_SPIN_YAW_RATE_REF 10.0f
#define STOOL_BRAKE_ACC_REF 16.0f #define STOOL_SPIN_YAW_ACC_REF 5.0f
#define STOOL_PITCH_K 50.0f #define STOOL_ACC_REF 4.0f
#define STOOL_PITCH_W_K 12.5f #define STOOL_BRAKE_ACC_REF 8.0f
#define STOOL_VEL_K 6.2f #define STOOL_PITCH_K 55.0f
#define STOOL_STOP_DIST_K 3.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_PITCH_REF -0.10f
#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
@@ -73,6 +75,7 @@ static float stool_vel;
static float stool_dist; static float stool_dist;
static float stool_stop_dist; static float stool_stop_dist;
static float stool_target_wz; static float stool_target_wz;
static uint8_t stool_spin_enabled;
static chassis_mode_e last_chassis_mode = CHASSIS_ZERO_FORCE; static chassis_mode_e last_chassis_mode = CHASSIS_ZERO_FORCE;
static void ClearMotionTargets(void) static void ClearMotionTargets(void)
@@ -80,6 +83,7 @@ static void ClearMotionTargets(void)
chassis.target_v = 0.0f; chassis.target_v = 0.0f;
chassis.target_dist = chassis.dist; chassis.target_dist = chassis.dist;
stool_target_wz = 0.0f; stool_target_wz = 0.0f;
stool_spin_enabled = 0;
} }
static void ClearControlOutput(void) static void ClearControlOutput(void)
@@ -337,6 +341,7 @@ static void ControlSwitch()
chassis_cmd_recv.loader_mode = LOAD_STOP; chassis_cmd_recv.loader_mode = LOAD_STOP;
chassis_cmd_recv.vision_mode = UNLOCK; chassis_cmd_recv.vision_mode = UNLOCK;
chassis_cmd_recv.ui_mode = UI_KEEP; chassis_cmd_recv.ui_mode = UI_KEEP;
stool_spin_enabled = 0;
if (!RemoteControlIsOnline()) if (!RemoteControlIsOnline())
{ {
@@ -357,7 +362,15 @@ static void ControlSwitch()
chassis_cmd_recv.chassis_mode = CHASSIS_STOOL_MODE; 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.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. */ /* Joint motors must remain disabled on this demo chassis. */
@@ -402,7 +415,10 @@ static void WokingStateSet()
EnableDrivenMotor(); EnableDrivenMotor();
chassis_status = ROBOT_READY; chassis_status = ROBOT_READY;
l_side.target_len = r_side.target_len = 0.12f; 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) if (fabsf(chassis_cmd_recv.vx) > 0.001f)
{ {