summaryrefslogtreecommitdiff
path: root/digital/io-hub/src
diff options
context:
space:
mode:
authorNicolas Schodet2013-04-22 23:31:27 +0200
committerNicolas Schodet2013-04-22 23:33:48 +0200
commit4e4768faf9fc78db9fe59c5b660dd31217e5a6b5 (patch)
tree09b7685588596ac7e5887e69affb1af968fe4c2e /digital/io-hub/src
parenteb45102670d7e8fade34cd3a45b392ce672eef34 (diff)
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.
Diffstat (limited to 'digital/io-hub/src')
-rw-r--r--digital/io-hub/src/common-cc/move.cc24
-rw-r--r--digital/io-hub/src/common-cc/move.hh2
2 files changed, 17 insertions, 9 deletions
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,