// goto_tracker.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_tracker.h" /// Constructeur. GotoTracker::GotoTracker (void) : track_ (false), changed_ (true), x_ (0.0), y_ (0.0), dx_ (0.0), dy_ (0.0) { } /// Initialise le Goto, appelé au début de la trajectoire. void GotoTracker::init (const Tracker &t) { } /// 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 GotoTracker::get (const Tracker &t, double distmax, double eps, double &dist, double &x, double &y) { if (changed_) { if (!track_) { return false; } else { double ca = cos (t.getAngle ()); double sa = sin (t.getAngle ()); x_ = t.getX () + dx_ * ca - dy_ * sa; y_ = t.getY () + dx_ * sa + dy_ * ca; } changed_ = false; } dist = t.getDistance (x_, y_); t.getPoint (x_, y_, x, y, distmax); return true; } /// Change la position de la cible. void GotoTracker::setTracked (bool track, double x, double y) { changed_ = true; track_ = track; dx_ = x; dy_ = y; }