From 7b61cbeddc510f88ad014dbb3c4fa6517f0766d3 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 8 Apr 2013 21:59:23 +0200 Subject: digital/io-hub/src/common-cc: give stop distance as parameter --- digital/io-hub/src/common-cc/obstacles.cc | 8 +++++--- digital/io-hub/src/common-cc/obstacles.hh | 5 +++-- 2 files changed, 8 insertions(+), 5 deletions(-) (limited to 'digital/io-hub/src/common-cc') diff --git a/digital/io-hub/src/common-cc/obstacles.cc b/digital/io-hub/src/common-cc/obstacles.cc index 882744e8..af6d1a79 100644 --- a/digital/io-hub/src/common-cc/obstacles.cc +++ b/digital/io-hub/src/common-cc/obstacles.cc @@ -98,8 +98,10 @@ Obstacles::add (const vect_t &pos) } bool -Obstacles::blocking (const vect_t &robot, const vect_t &dest) const +Obstacles::blocking (const vect_t &robot, const vect_t &dest, int stop_mm_) const { + if (stop_mm_ == -1) + stop_mm_ = stop_mm; // Stop here if no obstacle. bool obs_valid = false; for (int i = 0; !obs_valid && i < obstacles_nb_; i++) @@ -114,11 +116,11 @@ Obstacles::blocking (const vect_t &robot, const vect_t &dest) const // If destination is near, use clearance to destination point instead of // stop length. vect_t t; - if (d < stop_mm) + if (d < stop_mm_) t = dest; else { - vect_scale_f824 (&vd, (1ll << 24) / d * stop_mm); + vect_scale_f824 (&vd, (1ll << 24) / d * stop_mm_); t = robot; vect_translate (&t, &vd); } diff --git a/digital/io-hub/src/common-cc/obstacles.hh b/digital/io-hub/src/common-cc/obstacles.hh index 6b65dfc6..9a87f895 100644 --- a/digital/io-hub/src/common-cc/obstacles.hh +++ b/digital/io-hub/src/common-cc/obstacles.hh @@ -44,7 +44,8 @@ class Obstacles void add (const vect_t &pos); /// Return true if there is an obstacle near the robot while going to a /// destination point. - bool blocking (const vect_t &robot, const vect_t &dest) const; + bool blocking (const vect_t &robot, const vect_t &dest, int stop_mm_ = -1) + const; /// Setup obstacles in path finding object. void add_obstacles (Path &path) const; public: @@ -81,7 +82,7 @@ class Obstacles { /// Position. vect_t pos; - /// Validity period as a number of cycles, or 0 if unvalid. + /// Validity period as a number of cycles, or 0 if invalid. int valid; }; /// Number of obstacles. -- cgit v1.2.3