#ifndef tracker_h #define tracker_h // tracker.h // 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 "logger/log.h" #include #include /// Classe de suivi des coordonnée et de calcul d'erreur. class Tracker { /// Position et angle (mm et rad). double posX_, posY_, angle_; /// Distance entre les deux roues (mm). double f_; /// Nombre d'updates nuls consécutifs. int zero_; /// Anciène valeur des compteurs. double oldCountLeft_, oldCountRight_; /// Limite admissible pour les adjusts. double adjustXLimit_, adjustYLimit_, adjustAngleLimit_; // Système de log. mutable Log log_; public: /// Constructeur. Tracker (void); /// Destructeur. ~Tracker (void); /// Set the position. void setPos (double x, double y, double angle); /// Get the position. void getPos (double &x, double &y, double &angle) const; /// Get X position. double getX (void) const { return posX_; } /// Get Y position. double getY (void) const { return posY_; } /// Get angle. double getAngle (void) const { return angle_; } /// Ajuste les X. void adjustX (double x); /// Ajuste les Y. void adjustY (double y); /// Ajuste l'angle. void adjustAngle (double angle); /// Get distance. double getDistance (double x, double y) const; /// Récupère la distance à parcourir avec chaque roue pour parcourir un /// angle. double getAngleDistance (double angleDiff) const; /// Récupère les coordonnées d'un point à moins de dist (mm), dans la /// direction de (dx, dy). void getPoint (double dx, double dy, double &x, double &y, double dist) const; /// Retourne vrai si arrété. bool stopped (void) { return zero_ > 10; } /// Récupère l'angle entre l'angle courant et a normalisé entre -pi et pi. double getAngleDiff (double a) const; /// Calcule l'erreur linéaire et angulaire pour atteindre un point. /// (x, y) : point à atteindre (mm). /// el, ea : erreur linéaire et angulaire (-1..+1). /// a : angle. void computeError (double x, double y, double &el, double &ea, double &a) const; /// Calcule l'erreur angulaire pour atteindre un angle, retourne faux si /// atteind. /// a : angle à atteindre (rad). /// ea : erreur angulaire (-1..+1). /// eps : angle en dessous duquel on considère qu'on est arrivé (rad). bool computeAngleError (double a, double &ea, double eps) const; /// Calcule la longueur de l'arc gauche et droite pour atteindre un point. /// EPS précise la distance minimale. Renvois faux si atteind. /// \deprecated Pas vraiment l'algo qui faut. bool computeArcs (double x, double y, double &l, double &r, double eps) const; /// Met à jour la position (mm). void update (double l, double r, bool zero); }; /// Affiche la position. std::ostream &operator<< (std::ostream &os, const Tracker &t); /// Get distance. inline double Tracker::getDistance (double x, double y) const { double dx = x - posX_; double dy = y - posY_; return sqrt (dx * dx + dy * dy); } #endif // tracker_h