// 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 "tracker.h" #include "config/config.h" #include "utils/mathutil.h" /// Constructeur. Tracker::Tracker (void) : posX_ (0.0), posY_ (0.0), angle_ (0.0), f_ (10.0), zero_ (0), oldCountLeft_ (0.0), oldCountRight_ (0.0), adjustXLimit_ (120.0), adjustYLimit_ (120.0), adjustAngleLimit_ (M_PI/3/2), log_ ("tracker") { // Lit la conf. Config rc ("rc/tracker"); while (!rc.eof ()) { if (!( rc.get ("startx", posX_) || rc.get ("starty", posY_) || rc.get ("startangle", angle_) || rc.get ("adjustx", adjustXLimit_) || rc.get ("adjusty", adjustYLimit_) || rc.get ("adjustangle", adjustAngleLimit_) || rc.get ("footing", f_) )) rc.noId (); } } /// Destructeur. Tracker::~Tracker (void) { } /// Set the position. void Tracker::setPos (double x, double y, double angle) { posX_ = x; posY_ = y; angle_ = angle; } /// Get the position. void Tracker::getPos (double &x, double &y, double &angle) const { x = posX_; y = posY_; angle = angle_; } /// Ajuste les X. void Tracker::adjustX (double x) { if (fabs (posX_ - x) < adjustXLimit_) posX_ = x; } /// Ajuste les Y. void Tracker::adjustY (double y) { if (fabs (posY_ - y) < adjustYLimit_) posY_ = y; } /// Ajuste l'angle. void Tracker::adjustAngle (double angle) { if (fabs (getAngleDiff (angle)) < adjustAngleLimit_) angle_ = angle; } /// Récupère l'angle entre l'angle courant et a normalisé entre -pi et pi. double Tracker::getAngleDiff (double a) const { return angleNorm (angle_ - a); } /// Récupère la distance à parcourir avec chaque roue pour parcourir un angle. double Tracker::getAngleDistance (double angleDiff) const { return 0.5 * f_ * fabs (angleDiff); } /// Récupère les coordonnées d'un point à moins de dist (mm), dans la /// direction de (dx, dy). void Tracker::getPoint (double dx, double dy, double &x, double &y, double dist) const { // Calcule le delta. double dtx = dx - posX_; double dty = dy - posY_; // Test si trops loin. double sqd = dtx * dtx + dty * dty; if (sqd > dist * dist) { x = posX_ + dtx * dist / sqrt (sqd); y = posY_ + dty * dist / sqrt (sqd); } else { x = dx; y = dy; } } /// 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 Tracker::computeError (double x, double y, double &el, double &ea, double &a) const { // Calcule la différence avec la cible voulue. double eX = x - posX_; double eY = y - posY_; // Calcule la distance. double d = sqrt (eX * eX + eY * eY); // Ramène sur -1..+1 eX /= d; eY /= d; // Calcule sin et cos. double sa = sin (angle_); double ca = cos (angle_); // Calcule l'erreur linéaire (projection sur la direction courante). el = eX * ca + eY * sa; // Calcule l'erreur angulaire (projection sur la direction // perpendiculaire). ea = eX * -sa + eY * ca; // Calcule l'angle. a = ea > 0.0 ? acos (el) : -acos (el); } /// 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 Tracker::computeAngleError (double a, double &ea, double eps) const { ea = getAngleDiff (a); if (-eps < ea && ea < eps) { ea = 0.0; return false; } ea *= 1.0 / M_PI; return true; } /// 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 Tracker::computeArcs (double x, double y, double &l, double &r, double eps) const { // Vecteur vers le point à atteindre. double dX = x - posX_, dY = y - posY_; // Calcule la distance au point (x, y). double d = sqrt (dX * dX + dY * dY); if (d < eps) { l = r = 0.0; log_ (Log::verydebug) << "compute arc eps" << std::endl; return false; } // Calcule l'angle entre la direction courante et la direction du point à // atteindre, évite d'utiliser un atan. // Calcule le vecteur perpendiculaire au vecteur normal selon la // direction courante. double vX = -sin (angle_); double vY = cos (angle_); // Calcul de l'angle devant/deriere. if (dX * vY + dY * -vX < 0.0) { l = r = 0; log_ (Log::verydebug) << "compute arc back" << std::endl; return false; } // Calcul de l'angle par produit scalaire. double s = dX * vX + dY * vY; double dA = asin (s / d); // sin (a) = cos (a + pi/2) log_ (Log::verydebug) << "compute arc dA " << dA << " d " << d << std::endl; // Si l'angle est trops petit, on va tout droit, sinon /0. if (dA > -0.0001 && dA < 0.0001) { l = r = d; log_ (Log::verydebug) << "compute arc small angle" << std::endl; } // Si l'angle est trops grand, on ne fait pas le tour du monde. else if (dA > M_PI_2 || dA < -M_PI_2) { l = r = 0; log_ (Log::verydebug) << "compute arc big angle" << std::endl; return false; } else { // Calcule le rayon de courbure à suivre. double ro = 0.5 * d / sin (dA); // sin (dA) = cos (pi/2 - dA) // Rayon plus petit que l'empatement/2, stop. if (ro < 0.25 * f_ && ro > 0.25 * f_) { l = r = 0.0; log_ (Log::verydebug) << "compute arc small ro " << ro << std::endl; return false; } // L'angle du déplacement est le même que dA. l = (ro - 0.5 * f_) * dA; r = (ro + 0.5 * f_) * dA; log_ (Log::verydebug) << "compute arc ro " << ro << std::endl; } return true; } /// Met à jour la position (mm). void Tracker::update (double l, double r, bool zero) { double dL, dR; dL = l - oldCountLeft_; dR = r - oldCountRight_; oldCountLeft_ = l; oldCountRight_ = r; double oldAngle = angle_; angle_ += (dR - dL) / f_; // Compte les zeros. if (zero) { zero_++; } else { zero_ = 0; } // Calcule l'angle et l'avancement moyen. // Avec a petit (c'est le cas, car f_ >> abs (dR - dL)), a ~= atan (a). // En fait, non, on doit pas calculer l'arctangente... A vérifier. double dA = angle_ - oldAngle; double dS = 0.5 * (dL + dR); // log_ (Log::verydebug) << "update dL " << dL << " dR " << dR << " dA " << // dA << " dS " << dS << std::endl; // Si l'angle est petit, évite une division par presque 0. if (dA < 0.0001 && dA > -0.0001) { // Considère que l'on avance en ligne droite selon la moitié de // l'angle. double a = (angle_ + oldAngle) * 0.5; posX_ += dS * cos (a); posY_ += dS * sin (a); /*log_ (Log::verydebug) << "small dX " << dS * cos (a) << " dY " << dS * sin (a) << std::endl;*/ } else { // dS : arc de cercle parcouru (grâce à Thales). // R : rayon, dS = dA * R, R = dS / dA // On a plus qu'à calculer les dX, dY avec cos et sin. posX_ += (sin (angle_) - sin (oldAngle)) * dS / dA; posY_ += (cos (oldAngle) - cos (angle_)) * dS / dA; /*log_ (Log::verydebug) << "big dX " << (sin (angle_) - sin (oldAngle)) * dS / dA << " dY " << (cos (oldAngle) - cos (angle_)) * dS / dA << " R " << dS / dA << std::endl;*/ } log_ (Log::info) << "update pos " << *this << std::endl; } /// Affiche la position. std::ostream & operator<< (std::ostream &os, const Tracker &t) { os << t.getX () << ' ' << t.getY () << ' ' << t.getAngle (); return os; }