Skip to content

Commit

Permalink
WIP smoothing
Browse files Browse the repository at this point in the history
  • Loading branch information
@lukash
lukash committed Sep 23, 2024
1 parent 6166fc7 commit d239e6a
Show file tree
Hide file tree
Showing 3 changed files with 291 additions and 0 deletions.
164 changes: 164 additions & 0 deletions src/main.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -134,6 +134,29 @@ typedef struct {

FootpadSensor footpad_sensor;

bool test_pressed;
bool test_noise;
float test_target;

float test_value;
float test_ramped_step;

float test_ema_sfirst_value;
float test_ema_sfirst_v1;

float test_ema_slast_value;

float test_ema_2nd_v1;
float test_ema_2nd_value;

float test_ema_3rd_v1;
float test_ema_3rd_v2;
float test_ema_3rd_value;

float test_step_filter_value;
float test_step_filter_ramped_step;
float test_step_filter_smooth_target;

// Feature: Turntilt
float last_yaw_angle, yaw_angle, abs_yaw_change, last_yaw_change, yaw_change, yaw_aggregate;
float turntilt_boost_per_erpm, yaw_aggregate_target;
Expand Down Expand Up @@ -1228,6 +1251,147 @@ static void refloat_thd(void *arg) {

footpad_sensor_update(&d->footpad_sensor, &d->float_conf);

if (d->footpad_sensor.state != FS_NONE) {
if (d->footpad_sensor.state == FS_LEFT) {
d->test_noise = true;
} else {
d->test_noise = false;
}
d->test_pressed = true;
d->disengage_timer = d->current_time;

d->test_target = 0;

d->test_value = 0;
d->test_ramped_step = 0;

d->test_ema_sfirst_value = 0;
d->test_ema_sfirst_v1 = 0;

d->test_ema_slast_value = 0;

d->test_ema_2nd_v1 = 0;
d->test_ema_2nd_value = 0;

d->test_ema_3rd_v1 = 0;
d->test_ema_3rd_v2 = 0;
d->test_ema_3rd_value = 0;

d->test_step_filter_value = 0;
d->test_step_filter_ramped_step = 0;
d->test_step_filter_smooth_target = 0;

VESC_IF->plot_init("t", "value");
VESC_IF->plot_add_graph("target");
VESC_IF->plot_add_graph("orig smooth");
VESC_IF->plot_add_graph("rate limit then EMA");
VESC_IF->plot_add_graph("EMA rate ltd.");
// VESC_IF->plot_add_graph("2nd order EMA rate ltd.");
VESC_IF->plot_add_graph("3rd order EMA rate ltd.");
VESC_IF->plot_add_graph("EMA rate ltd. then EMA on step");
}

// if (d->footpad_sensor.state == FS_LEFT) {
// d->test_target += 0.1f;
// } else if (d->footpad_sensor.state == FS_RIGHT) {
// d->test_target -= 0.1f;
// }

if (d->test_pressed) {
float time = d->current_time - d->disengage_timer;
if (time < 0.2f) {
d->test_target = 5.0f;
} else if (time > 0.4f && time < 0.5f) {
d->test_target = 0.0f;
} else if (time > 0.8f && time < 1.0f) {
d->test_target += 0.02f;
} else if (time > 1.2f && time < 1.4f) {
d->test_target -= 0.01f;
} else if (time > 1.4f && time < 1.5f) {
d->test_target += 0.08f;
} else if (time > 1.5f && time < 1.6f) {
d->test_target -= 0.08f;
} else if (time > 1.6f && time < 1.7f) {
d->test_target += 0.08f;
} else if (time > 1.7f && time < 1.8f) {
d->test_target -= 0.08f;
} else if (time > 1.8f) {
d->test_pressed = false;
}

float target = d->test_target;
float noise = 0;
if (d->test_noise) {
noise = (float) rnd(time * 1e6) / (UINT32_MAX / 2) - 1.0f;
noise = -logf(1.01 - fabsf(noise)) * 0.2f * sign(noise);
target += noise;
}

uint8_t gi = 0;

VESC_IF->plot_set_graph(gi++);
VESC_IF->plot_send_points(time * 1000, target);
// VESC_IF->plot_send_points(time * 1000, d->test_target);

smooth_rampf(&d->test_value, &d->test_ramped_step, target, 0.07, 0.05, 1.5f);
VESC_IF->plot_set_graph(gi++);
VESC_IF->plot_send_points(time * 1000, d->test_value);

float alpha = 0.025f;
float higher_order_mult = 0.8f;

rampf_simple_step_first(
&d->test_ema_sfirst_value, &d->test_ema_sfirst_v1, target, 0.07, alpha * 2
);
VESC_IF->plot_set_graph(gi++);
VESC_IF->plot_send_points(time * 1000, d->test_ema_sfirst_value);

rampf_simple_step_last(&d->test_ema_slast_value, target, 0.07, alpha);
VESC_IF->plot_set_graph(gi++);
VESC_IF->plot_send_points(time * 1000, d->test_ema_slast_value);

// rampf_ema_2nd(
// &d->test_ema_2nd_value,
// &d->test_ema_2nd_v1,
// target,
// 0.07,
// alpha * 2 * higher_order_mult
//);
// VESC_IF->plot_set_graph(gi++);
// VESC_IF->plot_send_points(time * 1000, d->test_ema_2nd_value);

rampf_ema_3rd(
&d->test_ema_3rd_value,
&d->test_ema_3rd_v1,
&d->test_ema_3rd_v2,
target,
0.07,
alpha * 3 * higher_order_mult
);
VESC_IF->plot_set_graph(gi++);
VESC_IF->plot_send_points(time * 1000, d->test_ema_3rd_value);

// rampf_step_filter(
// &d->test_step_filter_value,
// &d->test_step_filter_ramped_step,
// target,
// 0.07,
// alpha * 1.8
//);
rampf_step_filter2(
&d->test_step_filter_value,
&d->test_step_filter_smooth_target,
&d->test_step_filter_ramped_step,
target,
0.07,
alpha * 2.2f
);
// VESC_IF->plot_set_graph(gi++);
// VESC_IF->plot_send_points(time * 1000, d->test_step_filter_smooth_target);
VESC_IF->plot_set_graph(gi++);
VESC_IF->plot_send_points(time * 1000, d->test_step_filter_value);
}

if (d->footpad_sensor.state == FS_NONE && d->state.state == STATE_RUNNING &&
d->state.mode != MODE_FLYWHEEL && d->motor.abs_erpm > d->switch_warn_beep_erpm) {
// If we're at riding speed and the switch is off => ALERT the user
Expand Down
105 changes: 105 additions & 0 deletions src/utils.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,6 +67,111 @@ void smooth_rampf(
}
}

void rampf_simple_step_first(
float *value, float *v1, float target, float step, float smoothing_factor
) {
float delta = target - *v1;

if (fabsf(delta) > step) {
delta = step * sign(delta);
}

*v1 += delta;

*value += smoothing_factor * (*v1 - *value);
}

void rampf_simple_step_last(float *value, float target, float step, float smoothing_factor) {
float delta = smoothing_factor * (target - *value);

if (fabsf(delta) > step) {
delta = step * sign(delta);
}

*value += delta;
}

void rampf_ema_2nd(float *value, float *v1, float target, float step, float smoothing_factor) {
*v1 += smoothing_factor * (target - *v1);

float delta = smoothing_factor * (*v1 - *value);

if (fabsf(delta) > step) {
delta = step * sign(delta);
}

*value += delta;
}

void rampf_ema_3rd(
float *value, float *v1, float *v2, float target, float step, float smoothing_factor
) {
*v1 += smoothing_factor * (target - *v1);
*v2 += smoothing_factor * (*v1 - *v2);

float delta = smoothing_factor * (*v2 - *value);

if (fabsf(delta) > step) {
delta = step * sign(delta);
}

*value += delta;
}

// Does EMA on the target, but applies a secondary EMA on the step of the first
// one (since EMA can be understood as adding the difference between target and
// value multiplied by alpha, and that's the ramped_step below). Only applies
// the secondary EMA when the step is increasing, not when it's decreasing. Has
// issues dealing with noisy signal, hence version 2 below.
//
// The secondary EMA is meant to ease the transition into a sharp target
// change. It can have a separate alpha which will determine the smoothness of
// this transition.
void rampf_step_filter(
float *value, float *ramped_step, float target, float step, float smoothing_factor
) {
float delta = smoothing_factor * (target - *value);

if (fabsf(delta) > step) {
delta = step * sign(delta);
}

if (fabsf(delta) > fabsf(*ramped_step)) {
*ramped_step += smoothing_factor * (delta - *ramped_step);
} else {
*ramped_step = delta;
}

*value += *ramped_step;
}

// A version of the above, which also filters the target value before applying
// the same filter. This (third) EMA could have another alpha of its own.
void rampf_step_filter2(
float *value,
float *smooth_target,
float *ramped_step,
float target,
float step,
float smoothing_factor
) {
*smooth_target += smoothing_factor * (target - *smooth_target);

float delta = smoothing_factor * 0.7f * (*smooth_target - *value);

if (fabsf(delta) > step) {
delta = step * sign(delta);
}

if (fabsf(delta) > fabsf(*ramped_step)) {
*ramped_step += smoothing_factor * (delta - *ramped_step);
} else {
*ramped_step = delta;
}

*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
22 changes: 22 additions & 0 deletions src/utils.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -127,3 +127,25 @@ void smooth_rampf(
float smoothing_factor,
float smooth_center_window
);

void rampf_simple_step_first(
float *value, float *v1, float target, float step, float smoothing_factor
);
void rampf_simple_step_last(float *value, float target, float step, float smoothing_factor);

void rampf_ema_2nd(float *value, float *v1, float target, float step, float smoothing_factor);
void rampf_ema_3rd(
float *value, float *v1, float *v2, float target, float step, float smoothing_factor
);

void rampf_step_filter(
float *value, float *ramped_step, float target, float step, float smoothing_factor
);
void rampf_step_filter2(
float *value,
float *smooth_target,
float *ramped_step,
float target,
float step,
float smoothing_factor
);

0 comments on commit d239e6a

Please sign in to comment.