From 94ad65245035203845f264f89a05d564b680869d Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 3 Apr 2013 08:16:40 +0200 Subject: digital/io-hub/src/apbirthday: add move FSM --- digital/io-hub/src/common-cc/obstacles.cc | 42 ++++++++++++++++++++++++++++++- 1 file changed, 41 insertions(+), 1 deletion(-) (limited to 'digital/io-hub/src/common-cc/obstacles.cc') diff --git a/digital/io-hub/src/common-cc/obstacles.cc b/digital/io-hub/src/common-cc/obstacles.cc index d98a92ba..1c02cce6 100644 --- a/digital/io-hub/src/common-cc/obstacles.cc +++ b/digital/io-hub/src/common-cc/obstacles.cc @@ -22,6 +22,7 @@ // // }}} #include "obstacles.hh" +#include "bot.hh" extern "C" { #include "modules/math/geometry/distance.h" @@ -99,7 +100,46 @@ Obstacles::add (const vect_t &pos) bool Obstacles::blocking (const vect_t &robot, const vect_t &dest) const { - // TODO + // Stop here if no obstacle. + bool obs_valid = false; + for (int i = 0; !obs_valid && i < obstacles_nb_; i++) + obs_valid = obstacles_[i].valid != 0; + if (!obs_valid) + return false; + // If destination is realy near, stop here. + vect_t vd = dest; vect_sub (&vd, &robot); + int d = vect_norm (&vd); + if (d < epsilon_mm) + return false; + // If destination is near, use clearance to destination point instead of + // stop length. + vect_t t; + if (d < stop_mm) + t = dest; + else + { + vect_scale_f824 (&vd, (1ll << 24) / d * stop_mm); + t = robot; + vect_translate (&t, &vd); + } + // Now, look at obstacles. + for (int i = 0; i < obstacles_nb_; i++) + { + if (!obstacles_[i].valid) + continue; + // Vector from robot to obstacle. + vect_t vo = obstacles_[i].pos; vect_sub (&vo, &robot); + // Ignore if in our back. + int dp = vect_dot_product (&vd, &vo); + if (dp < 0) + continue; + // Check distance. + int od = distance_segment_point (&robot, &t, &obstacles_[i].pos); + if (od > BOT_SIZE_SIDE + clearance_mm / 2 + obstacle_radius_mm) + continue; + // Else, obstacle is blocking. + return true; + } return false; } -- cgit v1.2.3