From bbc0bfccf74022a37e8afd312cfde802444e281c Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 1 Jun 2011 00:40:10 +0200 Subject: digital/io-hub: fix move revert ok --- digital/io-hub/src/robospierre/move.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'digital/io-hub/src/robospierre') diff --git a/digital/io-hub/src/robospierre/move.c b/digital/io-hub/src/robospierre/move.c index 09faf8f9..623dabdf 100644 --- a/digital/io-hub/src/robospierre/move.c +++ b/digital/io-hub/src/robospierre/move.c @@ -206,8 +206,9 @@ move_go_or_rotate (vect_t dst, uint16_t angle, uint8_t with_angle, uint16_t dst_angle = atan2 (v.y, v.x) * ((1l << 16) / (2 * M_PI)); if (backward & ASSERV_BACKWARD) dst_angle += 0x8000; + int16_t diff_angle = dst_angle - robot_angle; if ((backward & ASSERV_REVERT_OK) - && (dst_angle ^ robot_angle) & 0x8000) + && (diff_angle > 0x4000 || diff_angle < -0x4000)) dst_angle += 0x8000; int16_t diff = dst_angle - robot_angle; /* Move or rotate. */ -- cgit v1.2.3