diff --git a/application/chassis/balance.c b/application/chassis/balance.c index 3ef761f..611276b 100644 --- a/application/chassis/balance.c +++ b/application/chassis/balance.c @@ -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) {