// goto_circle.cc // nono - programme du robot 2004. {{{ // // Copyright (C) 2004 Nicolas Schodet // // Robot APB Team/Efrei 2004. // Web: http://assos.efrei.fr/robot/ // Email: robot AT efrei DOT fr // // This program is free software; you can redistribute it and/or modify // it under the terms of the GNU General Public License as published by // the Free Software Foundation; either version 2 of the License, or // (at your option) any later version. // // This program is distributed in the hope that it will be useful, // but WITHOUT ANY WARRANTY; without even the implied warranty of // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // GNU General Public License for more details. // // You should have received a copy of the GNU General Public License // along with this program; if not, write to the Free Software // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. // // }}} #include "goto_circle.h" #include "utils/mathutil.h" #include // XXX /// Constructeur. /// r : rayon (mm). /// a : angle (rad > 0). GotoCircle::GotoCircle (double r, double a) : r_ (r), a_ (a), cx_ (0.0), cy_ (0.0) { } /// Initialise le Goto, appelé au début de la trajectoire. void GotoCircle::init (const Tracker &t) { // Calcule le centre de rotation. double x, y, a; t.getPos (x, y, a); cx_ = x - sin (a) * r_; cy_ = y + cos (a) * r_; // Calcule l'angle de destination. a_ = a_ + a - M_PI * 0.5; std::cout << "goto: circle: init " << cx_ << ' ' << cy_ << ' ' << a_ << std::endl; } /// Fournit la distance au point final (mm), le point le plus loin à moins /// de distmax (mm) et renvois false si le Goto est fini. /// eps : distance (mm) en dessous de laquelle on considère que la destination /// est atteinte. /// distmax : distance (mm) maximale du point. /// dist : distance (mm) au point final. /// (x, y) : point intermédiaire (mm). bool GotoCircle::get (const Tracker &t, double distmax, double eps, double &dist, double &x, double &y) { double dx, dy; dx = t.getX () - cx_; dy = t.getY () - cy_; // Calcule l'angle et la distance avec le centre. double d, a; toPolair (dx, dy, d, a); std::cout << "goto: circle: polair " << d << ' ' << a << std::endl; // Calcule le point intermédiaire. double aInter, da; da = angleNorm (a_ - a); // Corection si angle négatif et que c'est pas cool. if (r_ > 0.0 && da < 0.0) { da = 2.0 * M_PI - da; } else if (r_ < 0.0 && da > 0.0) { da = -2.0 * M_PI + da; } std::cout << "goto: circle: dists " << d << ' ' << r_ << ' ' << distmax << std::endl; if (d > r_ + distmax || r_ > distmax + d) { // Pas d'intersection. std::cout << "goto: circle: miss" << std::endl; aInter = a; } else { // Intersection. double f, ia; f = (r_ * r_ - distmax * distmax + d * d) / (2.0 * d); ia = acos (f / r_); std::cout << "goto: circle: diff " << da << ' ' << ia << std::endl; if (da < ia && da > -ia) { // On est tout prés, renvois le point destination. aInter = a_; std::cout << "goto: circle: near" << std::endl; } else { std::cout << "goto: circle: far" << std::endl; // On est loin, renvois un point intermédiaire. if (r_ > 0.0) aInter = a + ia; else aInter = M_PI + a - ia; } } x = cx_ + r_ * cos (aInter); y = cy_ + r_ * sin (aInter); dist = da * r_; return dist > eps; }