From 5477bb94395cf2f592906f2fe0676ae8d9f383d7 Mon Sep 17 00:00:00 2001 From: Dado Mista Date: Fri, 26 Apr 2024 13:41:15 -0700 Subject: [PATCH 1/3] Smoothened ATR ramping (like inputtilt ramping) Following the example of inputtilt, ATR setpoints are now ramped slower when near the current setpoint. This allows for much higher ramping speeds without causing a jerking effect. Instead of using 5deg/sec and 3deg/sec one can now easily run 20 and 12 or even more without any perceived downsides. Feature: Smoothened/faster setpoint ramping for ATR Signed-off-by: Dado Mista --- src/atr.c | 63 ++++++++++++++++++++++++++++++++++++++++++------------- src/atr.h | 1 + 2 files changed, 49 insertions(+), 15 deletions(-) diff --git a/src/atr.c b/src/atr.c index 6cb6602..83636c8 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,65 @@ 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 + if (config->inputtilt_smoothing_factor > 0) { + float smoothing_factor = 0.05; + // Sets the angle away from Target that step size begins ramping down + float smooth_center_window = 1.5; + float tiltback_target_diff = atr->target_offset - atr->offset; + + // 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) + atr->ramped_step_size = (smoothing_factor * step_size * (tiltback_target_diff / 2)) + + ((1 - smoothing_factor) * atr->ramped_step_size); + // Linearly ramped down step size is provided as minimum to prevent overshoot + float centering_step_size = + fminf(fabsf(atr->ramped_step_size), fabsf(tiltback_target_diff / 2) * step_size) * + sign(tiltback_target_diff); + if (fabsf(tiltback_target_diff) < fabsf(centering_step_size)) { + atr->offset = atr->target_offset; + } else { + atr->offset += centering_step_size; + } + } else { + // Ramp up step size until the configured tilt speed is reached + atr->ramped_step_size = (smoothing_factor * step_size * sign(tiltback_target_diff)) + + ((1 - smoothing_factor) * atr->ramped_step_size); + atr->offset += atr->ramped_step_size; + } + } else { + rate_limitf(&atr->offset, atr->target_offset, step_size); + } } static void braketilt_update( @@ -225,9 +258,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; From 9542dc8d466cd2258f223ac23dc18c5651c61b5d Mon Sep 17 00:00:00 2001 From: Dado Mista Date: Sat, 27 Apr 2024 07:16:15 -0700 Subject: [PATCH 2/3] Smooth ramping for torque tilt too Following the example of inputtilt, TT setpoints are now ramped slower when near the current setpoint. This allows for much higher ramping speeds without causing a jerking effect. Instead of using 5deg/sec and 3deg/sec one can now easily run 20 and 12 or even more without any perceived downsides. Feature: Smoothened/faster setpoint ramping for Torque Tilt Signed-off-by: Dado Mista --- src/torque_tilt.c | 33 ++++++++++++++++++++++++++++++++- src/torque_tilt.h | 1 + 2 files changed, 33 insertions(+), 1 deletion(-) diff --git a/src/torque_tilt.c b/src/torque_tilt.c index 376d70a..59c52ea 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,37 @@ 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 + if (config->inputtilt_smoothing_factor > 0) { + float smoothing_factor = 0.04; + // Sets the angle away from Target that step size begins ramping down + float smooth_center_window = 1.5; + float tiltback_target_diff = target_offset - tt->offset; + + // 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) + tt->ramped_step_size = (smoothing_factor * step_size * (tiltback_target_diff / 2)) + + ((1 - smoothing_factor) * tt->ramped_step_size); + // Linearly ramped down step size is provided as minimum to prevent overshoot + float centering_step_size = + fminf(fabsf(tt->ramped_step_size), fabsf(tiltback_target_diff / 2) * step_size) * + sign(tiltback_target_diff); + if (fabsf(tiltback_target_diff) < fabsf(centering_step_size)) { + tt->offset = target_offset; + } else { + tt->offset += centering_step_size; + } + } else { + // Ramp up step size until the configured tilt speed is reached + tt->ramped_step_size = (smoothing_factor * step_size * sign(tiltback_target_diff)) + + ((1 - smoothing_factor) * tt->ramped_step_size); + tt->offset += tt->ramped_step_size; + } + } else { + rate_limitf(&tt->offset, target_offset, step_size); + } } 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; From dc97e54356e6d51c448d952e32788adb31cbc667 Mon Sep 17 00:00:00 2001 From: Dado Mista Date: Tue, 25 Jun 2024 13:27:21 -0700 Subject: [PATCH 3/3] Factor out smooth ramping into function Signed-off-by: Dado Mista --- src/atr.c | 31 +------------------------------ src/torque_tilt.c | 31 +------------------------------ src/utils.c | 34 ++++++++++++++++++++++++++++++++++ src/utils.h | 9 +++++++++ 4 files changed, 45 insertions(+), 60 deletions(-) diff --git a/src/atr.c b/src/atr.c index 83636c8..afce98a 100644 --- a/src/atr.c +++ b/src/atr.c @@ -200,36 +200,7 @@ static void atr_update(ATR *atr, const MotorData *motor, const RefloatConfig *co } // Smoothen changes in tilt angle by ramping the step size - if (config->inputtilt_smoothing_factor > 0) { - float smoothing_factor = 0.05; - // Sets the angle away from Target that step size begins ramping down - float smooth_center_window = 1.5; - float tiltback_target_diff = atr->target_offset - atr->offset; - - // 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) - atr->ramped_step_size = (smoothing_factor * step_size * (tiltback_target_diff / 2)) + - ((1 - smoothing_factor) * atr->ramped_step_size); - // Linearly ramped down step size is provided as minimum to prevent overshoot - float centering_step_size = - fminf(fabsf(atr->ramped_step_size), fabsf(tiltback_target_diff / 2) * step_size) * - sign(tiltback_target_diff); - if (fabsf(tiltback_target_diff) < fabsf(centering_step_size)) { - atr->offset = atr->target_offset; - } else { - atr->offset += centering_step_size; - } - } else { - // Ramp up step size until the configured tilt speed is reached - atr->ramped_step_size = (smoothing_factor * step_size * sign(tiltback_target_diff)) + - ((1 - smoothing_factor) * atr->ramped_step_size); - atr->offset += atr->ramped_step_size; - } - } else { - rate_limitf(&atr->offset, atr->target_offset, step_size); - } + smooth_rampf(&atr->offset, &atr->ramped_step_size, atr->target_offset, step_size, 0.05, 1.5); } static void braketilt_update( diff --git a/src/torque_tilt.c b/src/torque_tilt.c index 59c52ea..af0acab 100644 --- a/src/torque_tilt.c +++ b/src/torque_tilt.c @@ -62,36 +62,7 @@ void torque_tilt_update(TorqueTilt *tt, const MotorData *motor, const RefloatCon } // Smoothen changes in tilt angle by ramping the step size - if (config->inputtilt_smoothing_factor > 0) { - float smoothing_factor = 0.04; - // Sets the angle away from Target that step size begins ramping down - float smooth_center_window = 1.5; - float tiltback_target_diff = target_offset - tt->offset; - - // 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) - tt->ramped_step_size = (smoothing_factor * step_size * (tiltback_target_diff / 2)) + - ((1 - smoothing_factor) * tt->ramped_step_size); - // Linearly ramped down step size is provided as minimum to prevent overshoot - float centering_step_size = - fminf(fabsf(tt->ramped_step_size), fabsf(tiltback_target_diff / 2) * step_size) * - sign(tiltback_target_diff); - if (fabsf(tiltback_target_diff) < fabsf(centering_step_size)) { - tt->offset = target_offset; - } else { - tt->offset += centering_step_size; - } - } else { - // Ramp up step size until the configured tilt speed is reached - tt->ramped_step_size = (smoothing_factor * step_size * sign(tiltback_target_diff)) + - ((1 - smoothing_factor) * tt->ramped_step_size); - tt->offset += tt->ramped_step_size; - } - } else { - rate_limitf(&tt->offset, target_offset, 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/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 +);