summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2012-05-19 05:44:49 +0200
committerNicolas Schodet2012-05-19 05:44:49 +0200
commit508cad5f24c295d9cb628ebc7573b5cf2afacfa3 (patch)
treef7f013e0318eb92db8b1c428efa20d45958f1e4d
parenta0f115ece1275600e8e97f28bdd4c21eeb1650a5 (diff)
digital/avr/modules/motor/speed_control: fix problem with high accelerations
-rw-r--r--digital/avr/modules/motor/speed_control/speed_control.c8
1 files 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;