From 508cad5f24c295d9cb628ebc7573b5cf2afacfa3 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 19 May 2012 05:44:49 +0200 Subject: digital/avr/modules/motor/speed_control: fix problem with high accelerations --- digital/avr/modules/motor/speed_control/speed_control.c | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/digital/avr/modules/motor/speed_control/speed_control.c b/digital/avr/modules/motor/speed_control/speed_control.c index c13b4614..bdb3d375 100644 --- a/digital/avr/modules/motor/speed_control/speed_control.c +++ b/digital/avr/modules/motor/speed_control/speed_control.c @@ -58,13 +58,17 @@ speed_control_compute_max_speed_f (int32_t d, int32_t cur_f, int16_t acc_f, int16_t max) { int16_t s; - int32_t s_f; + int32_t d_abs, s_f; /* Compute maximum speed in order to be able to brake in time. * The "+ 0xff" is to ceil result. * s = sqrt (2 * a * d) */ - s = fixed_sqrt_ui32 ((2 * UTILS_ABS (d) * acc_f + 0xff) >> 8); + d_abs = UTILS_ABS (d); + s = fixed_sqrt_ui32 ((2 * d_abs * acc_f + 0xff) >> 8); /* Apply consign. */ s = UTILS_MIN (max, s); + /* Check it do not overshoot distance. */ + if (s > d_abs) + s = d_abs; /* Apply sign. */ if (d < 0) s = -s; -- cgit v1.2.3