From 546594731c6c7d2727382154a991a564b8b8d6da Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 31 May 2011 22:28:36 +0200 Subject: digital/io-hub: fix move backward --- digital/io-hub/src/robospierre/move.c | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) (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 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) { -- cgit v1.2.3