diff --git a/src/atr.c b/src/atr.c index 6cb6602..afce98a 100644 --- a/src/atr.c +++ b/src/atr.c @@ -48,6 +48,9 @@ void atr_configure(ATR *atr, const RefloatConfig *config) { // incorporate negative sign into braketilt factor instead of adding it each balance loop atr->braketilt_factor = -(0.5f + (20 - config->braketilt_strength) / 5.0f); } + + // Allows smoothing of Tilt + atr->ramped_step_size = 0; } static void atr_update(ATR *atr, const MotorData *motor, const RefloatConfig *config) { @@ -133,34 +136,34 @@ static void atr_update(ATR *atr, const MotorData *motor, const RefloatConfig *co // Key to keeping the board level and consistent is to determine the appropriate step size! // We want to react quickly to changes, but we don't want to overreact to glitches in // acceleration data or trigger oscillations... - float atr_step_size = 0; + float step_size = 0; const float TT_BOOST_MARGIN = 2; if (forward) { if (atr->offset < 0) { // downhill if (atr->offset < atr->target_offset) { // to avoid oscillations we go down slower than we go up - atr_step_size = atr->off_step_size; + step_size = atr->off_step_size; if ((atr->target_offset > 0) && ((atr->target_offset - atr->offset) > TT_BOOST_MARGIN) && motor->abs_erpm > 2000) { // boost the speed if tilt target has reversed (and if there's a significant // margin) - atr_step_size = atr->off_step_size * config->atr_transition_boost; + step_size = atr->off_step_size * config->atr_transition_boost; } } else { // ATR is increasing - atr_step_size = atr->on_step_size * response_boost; + step_size = atr->on_step_size * response_boost; } } else { // uphill or other heavy resistance (grass, mud, etc) if ((atr->target_offset > -3) && (atr->offset > atr->target_offset)) { // ATR winding down (current ATR is bigger than the target) // normal wind down case: to avoid oscillations we go down slower than we go up - atr_step_size = atr->off_step_size; + step_size = atr->off_step_size; } else { // standard case of increasing ATR - atr_step_size = atr->on_step_size * response_boost; + step_size = atr->on_step_size * response_boost; } } } else { @@ -168,35 +171,36 @@ static void atr_update(ATR *atr, const MotorData *motor, const RefloatConfig *co // downhill if (atr->offset > atr->target_offset) { // to avoid oscillations we go down slower than we go up - atr_step_size = atr->off_step_size; + step_size = atr->off_step_size; if ((atr->target_offset < 0) && ((atr->offset - atr->target_offset) > TT_BOOST_MARGIN) && motor->abs_erpm > 2000) { // boost the speed if tilt target has reversed (and if there's a significant // margin) - atr_step_size = atr->off_step_size * config->atr_transition_boost; + step_size = atr->off_step_size * config->atr_transition_boost; } } else { // ATR is increasing - atr_step_size = atr->on_step_size * response_boost; + step_size = atr->on_step_size * response_boost; } } else { // uphill or other heavy resistance (grass, mud, etc) if ((atr->target_offset < 3) && (atr->offset < atr->target_offset)) { // normal wind down case: to avoid oscillations we go down slower than we go up - atr_step_size = atr->off_step_size; + step_size = atr->off_step_size; } else { // standard case of increasing torquetilt - atr_step_size = atr->on_step_size * response_boost; + step_size = atr->on_step_size * response_boost; } } } if (motor->abs_erpm < 500) { - atr_step_size /= 2; + step_size /= 2; } - rate_limitf(&atr->offset, atr->target_offset, atr_step_size); + // Smoothen changes in tilt angle by ramping the step size + smooth_rampf(&atr->offset, &atr->ramped_step_size, atr->target_offset, step_size, 0.05, 1.5); } static void braketilt_update( @@ -225,9 +229,9 @@ static void braketilt_update( float braketilt_step_size = atr->off_step_size / config->braketilt_lingering; if (fabsf(atr->braketilt_target_offset) > fabsf(atr->braketilt_offset)) { - braketilt_step_size = atr->on_step_size * 1.5; - } else if (motor->abs_erpm < 800) { braketilt_step_size = atr->on_step_size; + } else if (motor->abs_erpm < 800) { + braketilt_step_size = atr->on_step_size * 0.5; } if (motor->abs_erpm < 500) { diff --git a/src/atr.h b/src/atr.h index c0f6a21..4498ba6 100644 --- a/src/atr.h +++ b/src/atr.h @@ -25,6 +25,7 @@ typedef struct { float on_step_size; float off_step_size; + float ramped_step_size; float accel_diff; float speed_boost; diff --git a/src/torque_tilt.c b/src/torque_tilt.c index 376d70a..af0acab 100644 --- a/src/torque_tilt.c +++ b/src/torque_tilt.c @@ -29,6 +29,7 @@ void torque_tilt_reset(TorqueTilt *tt) { void torque_tilt_configure(TorqueTilt *tt, const RefloatConfig *config) { tt->on_step_size = config->torquetilt_on_speed / config->hertz; tt->off_step_size = config->torquetilt_off_speed / config->hertz; + tt->ramped_step_size = 0; } void torque_tilt_update(TorqueTilt *tt, const MotorData *motor, const RefloatConfig *config) { @@ -60,7 +61,8 @@ void torque_tilt_update(TorqueTilt *tt, const MotorData *motor, const RefloatCon step_size /= 2; } - rate_limitf(&tt->offset, target_offset, step_size); + // Smoothen changes in tilt angle by ramping the step size + smooth_rampf(&tt->offset, &tt->ramped_step_size, target_offset, step_size, 0.04, 1.5); } void torque_tilt_winddown(TorqueTilt *tt) { diff --git a/src/torque_tilt.h b/src/torque_tilt.h index 4dea289..8c689a5 100644 --- a/src/torque_tilt.h +++ b/src/torque_tilt.h @@ -24,6 +24,7 @@ typedef struct { float on_step_size; float off_step_size; + float ramped_step_size; float offset; // rate-limited setpoint offset } TorqueTilt; diff --git a/src/utils.c b/src/utils.c index 1537384..9cda310 100644 --- a/src/utils.c +++ b/src/utils.c @@ -33,6 +33,40 @@ void rate_limitf(float *value, float target, float step) { } } +// Smoothen changes in tilt angle by ramping the step size +// smooth_center_window: Sets the angle away from Target that step size begins ramping down +void smooth_rampf( + float *value, + float *ramped_step, + float target, + float step, + float smoothing_factor, + float smooth_center_window +) { + float tiltback_target_diff = target - *value; + + // Within X degrees of Target Angle, start ramping down step size + if (fabsf(tiltback_target_diff) < smooth_center_window) { + // Target step size is reduced the closer to center you are (needed for smoothly + // transitioning away from center) + *ramped_step = (smoothing_factor * step * (tiltback_target_diff / 2)) + + ((1 - smoothing_factor) * *ramped_step); + // Linearly ramped down step size is provided as minimum to prevent overshoot + float centering_step = fminf(fabsf(*ramped_step), fabsf(tiltback_target_diff / 2) * step) * + sign(tiltback_target_diff); + if (fabsf(tiltback_target_diff) < fabsf(centering_step)) { + *value = target; + } else { + *value += centering_step; + } + } else { + // Ramp up step size until the configured tilt speed is reached + *ramped_step = (smoothing_factor * step * sign(tiltback_target_diff)) + + ((1 - smoothing_factor) * *ramped_step); + *value += *ramped_step; + } +} + float clampf(float value, float min, float max) { const float m = value < min ? min : value; return m > max ? max : m; diff --git a/src/utils.h b/src/utils.h index d22f4c5..dd6612e 100644 --- a/src/utils.h +++ b/src/utils.h @@ -97,3 +97,12 @@ float clampf(float value, float min, float max); * @param step A maximum unit of change of @p value. */ void rate_limitf(float *value, float target, float step); + +void smooth_rampf( + float *value, + float *ramped_step_size, + float target, + float step, + float smoothing_factor, + float smooth_center_window +);