From 4e4768faf9fc78db9fe59c5b660dd31217e5a6b5 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 22 Apr 2013 23:31:27 +0200 Subject: digital/io-hub/src/apbirthday: add minimum distance for move FSM Under this minimum distance, do not point the robot into the destination direction, this is meaningless. --- digital/io-hub/src/common-cc/move.cc | 24 +++++++++++++++--------- digital/io-hub/src/common-cc/move.hh | 2 ++ 2 files changed, 17 insertions(+), 9 deletions(-) (limited to 'digital') diff --git a/digital/io-hub/src/common-cc/move.cc b/digital/io-hub/src/common-cc/move.cc index bdbd3da0..75613584 100644 --- a/digital/io-hub/src/common-cc/move.cc +++ b/digital/io-hub/src/common-cc/move.cc @@ -147,16 +147,22 @@ Move::go_or_rotate (const vect_t &dst, uint16_t angle, bool with_angle, step_angle_ = angle; step_with_angle_ = with_angle; step_direction_consign_ = direction_consign; - // Compute angle to destination. + // Compute angle to destination, if destination is really near, angle is + // almost meaningless. vect_t v = dst; vect_sub (&v, &robot_position.v); - uint16_t dst_angle = std::atan2 (v.y, v.x) * ((1l << 16) / (2 * M_PI)); - if (direction_consign & Asserv::BACKWARD) - dst_angle += 0x8000; - int16_t diff_angle = dst_angle - robot_angle; - if ((direction_consign & Asserv::REVERT_OK) - && (diff_angle > 0x4000 || diff_angle < -0x4000)) - dst_angle += 0x8000; - int16_t diff = dst_angle - robot_angle; + uint16_t dst_angle = 0; + int16_t diff = 0; + if (vect_dot_product (&v, &v) > eps * eps) + { + dst_angle = std::atan2 (v.y, v.x) * ((1l << 16) / (2 * M_PI)); + if (direction_consign & Asserv::BACKWARD) + dst_angle += 0x8000; + int16_t diff_angle = dst_angle - robot_angle; + if ((direction_consign & Asserv::REVERT_OK) + && (diff_angle > 0x4000 || diff_angle < -0x4000)) + dst_angle += 0x8000; + diff = dst_angle - robot_angle; + } // Move or rotate. if (std::abs (diff) < 0x1000) { diff --git a/digital/io-hub/src/common-cc/move.hh b/digital/io-hub/src/common-cc/move.hh index c561d763..3f34f867 100644 --- a/digital/io-hub/src/common-cc/move.hh +++ b/digital/io-hub/src/common-cc/move.hh @@ -36,6 +36,8 @@ class Move LINEAR, ANGULAR, }; + /// Distance considered too small to move. + static const int eps = 15; public: /// Go to a position. void start (const Position &position, -- cgit v1.2.3