From 5ea1efc333fb48bb5ff32046a0e89ae1557c75ab Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 18 Apr 2008 10:32:39 +0200 Subject: * digital/asserv/src/asserv: - fixed glitch at end of trajectories. --- digital/asserv/src/asserv/speed.c | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) (limited to 'digital/asserv/src/asserv/speed.c') diff --git a/digital/asserv/src/asserv/speed.c b/digital/asserv/src/asserv/speed.c index a3bb6666..6f180a05 100644 --- a/digital/asserv/src/asserv/speed.c +++ b/digital/asserv/src/asserv/speed.c @@ -63,8 +63,9 @@ speed_compute_max_speed (int32_t d, int16_t cur, int16_t acc, int8_t max) { int16_t s; /* 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) >> 8); + s = fixed_sqrt_ui32 ((2 * UTILS_ABS (d) * acc + 0xff) >> 8); /* Apply consign. */ s = UTILS_MIN (max, s); /* Apply sign. */ @@ -81,11 +82,8 @@ static void speed_update_by_position (struct speed_t *speed, struct pos_t *pos) { int32_t diff = speed->pos_cons - pos->cons; - if (diff >= -speed->max && diff <= speed->max) - speed->cur = diff << 8; - else - speed->cur = speed_compute_max_speed (diff, speed->cur, speed->acc, - speed->max); + speed->cur = speed_compute_max_speed (diff, speed->cur, speed->acc, + speed->max); } /** Update shaft position consign according to its consign type. */ -- cgit v1.2.3