summaryrefslogtreecommitdiff
path: root/2004/i/nono/src/motor/tracker.cc
diff options
context:
space:
mode:
Diffstat (limited to '2004/i/nono/src/motor/tracker.cc')
-rw-r--r--2004/i/nono/src/motor/tracker.cc76
1 files changed, 53 insertions, 23 deletions
diff --git a/2004/i/nono/src/motor/tracker.cc b/2004/i/nono/src/motor/tracker.cc
index 4bd0e7b..a87beaf 100644
--- a/2004/i/nono/src/motor/tracker.cc
+++ b/2004/i/nono/src/motor/tracker.cc
@@ -29,7 +29,10 @@
/// Constructeur.
Tracker::Tracker (void)
: posX_ (0.0), posY_ (0.0), angle_ (0.0),
- f_ (10.0), zero_ (0), log_ ("tracker")
+ 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");
@@ -39,6 +42,9 @@ Tracker::Tracker (void)
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 ();
@@ -68,6 +74,30 @@ Tracker::getPos (double &x, double &y, double &angle) const
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
@@ -219,55 +249,55 @@ Tracker::computeArcs (double x, double y, double &l, double &r, double eps)
return true;
}
-/// Met à jour la position.
+/// Met à jour la position (mm).
void
-Tracker::update (double dL, double dR, bool zero)
+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 if (dL == 0.0 && dR == 0.0)
- {
- zero_++;
- return;
- }
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).
- double dA = (dR - dL) / f_;
+ // 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;
+// 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_ + 0.5 * dA;
+ double a = (angle_ + oldAngle) * 0.5;
posX_ += dS * cos (a);
posY_ += dS * sin (a);
- angle_ += dA;
- log_ (Log::verydebug) << "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
{
// 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.
- double oldA = angle_;
- angle_ += dA;
- posX_ += (sin (angle_) - sin (oldA)) * dS / dA;
- posY_ += (cos (oldA) - cos (angle_)) * dS / dA;
- log_ (Log::verydebug) << "big dX " <<
- -(sin (oldA) - sin (angle_)) * dS / dA <<
- " dY " << -(cos (oldA) - cos (angle_)) * dS / dA <<
- " R " << dS / dA << std::endl;
+ 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;
}