summaryrefslogtreecommitdiff
path: root/2004/i/nono/src/motor/tracker.cc
diff options
context:
space:
mode:
authorschodet2004-04-16 16:46:46 +0000
committerschodet2004-04-16 16:46:46 +0000
commita3a3a4d6d183b22036a606cda8553192c98321cd (patch)
tree37d158408eabfbb8524d1214a985a44e376ee333 /2004/i/nono/src/motor/tracker.cc
parente68b824329a537eddddd5b59b9bdf498698ee604 (diff)
Logger fonctionne.
MovementRotation aussi. Ajout de Serial::wait. Ajout de commentaires.
Diffstat (limited to '2004/i/nono/src/motor/tracker.cc')
-rw-r--r--2004/i/nono/src/motor/tracker.cc84
1 files changed, 57 insertions, 27 deletions
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 <iostream>
#include <math.h>
-// 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)
{