summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2008-04-18 10:32:39 +0200
committerNicolas Schodet2008-04-18 10:32:39 +0200
commit5ea1efc333fb48bb5ff32046a0e89ae1557c75ab (patch)
treef34df173a1714c2ff4f7d8d194e500dff8b18b00
parentaee65133f5500aa3659297f156786122668ffa66 (diff)
* digital/asserv/src/asserv:
- fixed glitch at end of trajectories.
-rw-r--r--digital/asserv/src/asserv/speed.c10
1 files changed, 4 insertions, 6 deletions
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. */