summaryrefslogtreecommitdiff
path: root/digital/io-hub/src/robospierre
diff options
context:
space:
mode:
authorNicolas Schodet2011-05-31 22:28:36 +0200
committerNicolas Schodet2011-05-31 22:44:18 +0200
commit546594731c6c7d2727382154a991a564b8b8d6da (patch)
treebb9bb1cba9fdc68123f7706549bc45e0c13cbfe0 /digital/io-hub/src/robospierre
parent280b556d985801e1528119ed87ac12f563d90745 (diff)
digital/io-hub: fix move backward
Diffstat (limited to 'digital/io-hub/src/robospierre')
-rw-r--r--digital/io-hub/src/robospierre/move.c9
1 files changed, 6 insertions, 3 deletions
diff --git a/digital/io-hub/src/robospierre/move.c b/digital/io-hub/src/robospierre/move.c
index e89e6d6b..6c204f61 100644
--- a/digital/io-hub/src/robospierre/move.c
+++ b/digital/io-hub/src/robospierre/move.c
@@ -187,21 +187,24 @@ move_go_or_rotate (vect_t dst, uint16_t angle, uint8_t with_angle,
uint8_t backward)
{
position_t robot_position;
+ asserv_get_position (&robot_position);
+ uint16_t robot_angle = robot_position.a;
+ if (backward & ASSERV_BACKWARD)
+ robot_angle += 0x8000;
/* Remember step. */
move_data.step = dst;
move_data.step_angle = angle;
move_data.step_with_angle = with_angle;
move_data.step_backward = backward;
/* Compute angle to destination. */
- asserv_get_position (&robot_position);
vect_t v = dst; vect_sub (&v, &robot_position.v);
uint16_t dst_angle = atan2 (v.y, v.x) * ((1l << 16) / (2 * M_PI));
if (backward & ASSERV_BACKWARD)
dst_angle += 0x8000;
if ((backward & ASSERV_REVERT_OK)
- && (dst_angle ^ robot_position.a) & 0x8000)
+ && (dst_angle ^ robot_angle) & 0x8000)
dst_angle += 0x8000;
- int16_t diff = dst_angle - robot_position.a;
+ int16_t diff = dst_angle - robot_angle;
/* Move or rotate. */
if (UTILS_ABS (diff) < 0x1000)
{