From a3a3a4d6d183b22036a606cda8553192c98321cd Mon Sep 17 00:00:00 2001 From: schodet Date: Fri, 16 Apr 2004 16:46:46 +0000 Subject: Logger fonctionne. MovementRotation aussi. Ajout de Serial::wait. Ajout de commentaires. --- 2004/i/nono/src/motor/tracker.cc | 84 +++++++++++++++++++++++++++------------- 1 file changed, 57 insertions(+), 27 deletions(-) (limited to '2004/i/nono/src/motor/tracker.cc') diff --git a/2004/i/nono/src/motor/tracker.cc b/2004/i/nono/src/motor/tracker.cc index 25f6f22..e5779da 100644 --- a/2004/i/nono/src/motor/tracker.cc +++ b/2004/i/nono/src/motor/tracker.cc @@ -25,13 +25,12 @@ #include "tracker.h" #include "config/config.h" -#include #include -// Constructeur. +/// Constructeur. Tracker::Tracker (void) : posX_ (0.0), posY_ (0.0), angle_ (0.0), - f_ (10.0), zero_ (0) + f_ (10.0), zero_ (0), log_ ("tracker") { // Lit la conf. Config rc ("rc/tracker"); @@ -47,12 +46,12 @@ Tracker::Tracker (void) } } -// Destructeur. +/// Destructeur. Tracker::~Tracker (void) { } -// Set the position. +/// Set the position. void Tracker::setPos (double x, double y, double angle) { @@ -61,7 +60,7 @@ Tracker::setPos (double x, double y, double angle) angle_ = angle; } -// Get the position. +/// Get the position. void Tracker::getPos (double &x, double &y, double &angle) const { @@ -70,8 +69,24 @@ Tracker::getPos (double &x, double &y, double &angle) const angle = angle_; } -// Calcule l'erreur linéaire et angulaire pour atteindre un point, -// retourne faux si atteind. +/// Récupère l'angle entre l'angle courant et a normalisé entre -pi et pi. +double +Tracker::getAngleDiff (double a) const +{ + double diff = angle_ - a; + while (diff > M_PI) + diff -= M_PI; + while (diff < -M_PI) + diff += M_PI; + return diff; +} + +/// Calcule l'erreur linéaire et angulaire pour atteindre un point, +/// retourne faux si atteind. +/// (x, y) : point à atteindre (mm). +/// el, ea : erreur linéaire et angulaire (-1..+1). +/// eps : distance en dessous de laquelle on considère qu'on est arrivé (mm). +/// dist : Distance maximale du point de référence (mm). bool Tracker::computeError (double x, double y, double &el, double &ea, double eps, double dist) const @@ -95,6 +110,9 @@ Tracker::computeError (double x, double y, double &el, double &ea, double eps, eX = eX / ds * dist; eY = eY / ds * dist; } + // Ramène sur -1..+1 + eX /= dist; + eY /= dist; // Calcule l'erreur linéaire (projection sur la direction courante). el = eX * cos (angle_) + eY * sin (angle_); // Calcule l'erreur angulaire (projection sur la direction @@ -103,22 +121,27 @@ Tracker::computeError (double x, double y, double &el, double &ea, double eps, return true; } -// Calcule l'erreur angulaire pour atteindre un angle, retourne faux si -// atteind. +/// 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 = a - angle_; - if (ea < eps) + 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. +/// 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 @@ -130,7 +153,7 @@ Tracker::computeArcs (double x, double y, double &l, double &r, double eps) if (d < eps) { l = r = 0.0; -std::cout << "compute arc eps" << std::endl; + log_ (Log::verydebug) << "compute arc eps" << std::endl; return false; } // Calcule l'angle entre la direction courante et la direction du point à @@ -143,24 +166,25 @@ std::cout << "compute arc eps" << std::endl; if (dX * vY + dY * -vX < 0.0) { l = r = 0; -std::cout << "compute arc back" << std::endl; + 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) -std::cout << "compute arc dA " << dA << " d " << d << std::endl; + 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; -std::cout << "compute arc small angle" << std::endl; + 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; -std::cout << "compute arc big angle" << std::endl; + log_ (Log::verydebug) << "compute arc big angle" << std::endl; return false; } else @@ -171,18 +195,19 @@ std::cout << "compute arc big angle" << std::endl; if (ro < 0.25 * f_ && ro > 0.25 * f_) { l = r = 0.0; -std::cout << "compute arc small ro " << ro << std::endl; + 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; -std::cout << "compute arc ro " << ro << std::endl; + log_ (Log::verydebug) << "compute arc ro " << ro << std::endl; } return true; } -// Met à jour la position. +/// Met à jour la position. void Tracker::update (double dL, double dR) { @@ -200,7 +225,8 @@ Tracker::update (double dL, double dR) // Avec a petit (c'est le cas, car f_ >> abs (dR - dL)), a ~= atan (a). double dA = (dR - dL) / f_; double dS = 0.5 * (dL + dR); -//std::cout << "tracker update dL " << dL << " dR " << dR << " dA " << dA << " dS " << dS << std::endl; + 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) { @@ -210,7 +236,8 @@ Tracker::update (double dL, double dR) posX_ += dS * cos (a); posY_ += dS * sin (a); angle_ += dA; -//std::cout << "tracker small dX " << dS * cos (a) << " dY " << dS * sin (a) << std::endl; + log_ (Log::verydebug) << "small dX " << dS * cos (a) << " dY " + << dS * sin (a) << std::endl; } else { @@ -221,12 +248,15 @@ Tracker::update (double dL, double dR) angle_ += dA; posX_ += (sin (angle_) - sin (oldA)) * dS / dA; posY_ += (cos (oldA) - cos (angle_)) * dS / dA; -//std::cout << "tracker big dX " << -(sin (oldA) - sin (angle_)) * dS / dA << " dY " << -(cos (oldA) - cos (angle_)) * dS / dA << " R " << dS / dA << std::endl; + log_ (Log::verydebug) << "big dX " << + -(sin (oldA) - sin (angle_)) * dS / dA << + " dY " << -(cos (oldA) - cos (angle_)) * dS / dA << + " R " << dS / dA << std::endl; } -std::cout << "update pos " << *this << std::endl; + log_ (Log::info) << "update pos " << *this << std::endl; } -// Affiche la position. +/// Affiche la position. std::ostream & operator<< (std::ostream &os, const Tracker &t) { -- cgit v1.2.3