Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Smooth ramping of ATR and Torque Tilt #2

Open
wants to merge 3 commits into
base: main
Choose a base branch
from
Open

Smooth ramping of ATR and Torque Tilt #2

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
5477bb9
Smoothened ATR ramping (like inputtilt ramping)
surfdado Apr 26, 2024
9542dc8
Smooth ramping for torque tilt too
surfdado Apr 27, 2024
dc97e54
Factor out smooth ramping into function
surfdado Jun 25, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
34 changes: 19 additions & 15 deletions src/atr.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down Expand Up @@ -133,70 +136,71 @@ 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 {
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->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(
Expand Down Expand Up @@ -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) {
Expand Down
1 change: 1 addition & 0 deletions src/atr.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@
typedef struct {
float on_step_size;
float off_step_size;
float ramped_step_size;

float accel_diff;
float speed_boost;
Expand Down
4 changes: 3 additions & 1 deletion src/torque_tilt.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down Expand Up @@ -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) {
Expand Down
1 change: 1 addition & 0 deletions src/torque_tilt.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
34 changes: 34 additions & 0 deletions src/utils.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Copy link
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm not really great at math, so I may totally be missing the point, but I find this computation overly complicated and ultimately flawed.

The step here is no longer the step in itself, it's a multiplier for the tiltback_target_diff and the step is thus a linear function of the diff. The way I understand it you are filtering towards the real step (ramped_step) if outside the window and you're filtering towards half of the real step when within the window.

The line above is what I think is not working really well, consider the case when you've just entered the smooth_center_window after you've filtered almost towards step * tiltback_target_diff, so that will be the value of *ramped_step. centering_step, though, after going through the fminf(), will be fabsf(tiltback_target_diff / 2) * step, which is almost half of that, so that transition won't be smooth at all. Or am I wrong on this?

I don't dispute it's working in a way (not sure why the above non-continuity doesn't noticeably manifest), but I want the math in the package to be solid. I think this is supposed to do in essence what @aeraglyx described in #2 (comment), except his solution I think is (to the extent I can make that claim, again, I'm not really too good at math) mathematically correct. There are likely more ways of doing this too, e.g. I was thinking of maybe an exponential function (while this one is linear), etc.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I have to admit that I simply copy pasted this code without questioning it from the Input-tilt implementation. Now that you forced me to look into it I have to say that something does look fishy about that code, good job for catching that.

Before further investigating I double-checked that I didn't introduce the problem during refactoring. Thus the problem should also affect the original input tilt implementation.

I think the problem starts when ramped_step_size is updated for the smooth center window. If the smooth center window is made large enough (>> 2), it appears as if ramped_step_size could actually temporarily increase instead of decreasing, since "tiltback_target_diff / 2" can be larger than 1.

I haven't had time yet to look into a fix but I figured I owed you a response acknowledging the problem you found.

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;
Expand Down
9 changes: 9 additions & 0 deletions src/utils.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
);
Loading