summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2011-06-01 00:40:10 +0200
committerNicolas Schodet2011-06-01 00:40:10 +0200
commitbbc0bfccf74022a37e8afd312cfde802444e281c (patch)
tree55d0a3800a724d1d1311d292b5e87f4a09601181
parent9e61ccb17af699691cd2e16c102d81cda2e119d5 (diff)
digital/io-hub: fix move revert ok
-rw-r--r--digital/io-hub/src/robospierre/move.c3
1 files changed, 2 insertions, 1 deletions
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. */