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.cc15
1 files changed, 13 insertions, 2 deletions
diff --git a/2004/i/nono/src/motor/tracker.cc b/2004/i/nono/src/motor/tracker.cc
index 381ed32..4bd0e7b 100644
--- a/2004/i/nono/src/motor/tracker.cc
+++ b/2004/i/nono/src/motor/tracker.cc
@@ -75,6 +75,13 @@ 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
@@ -101,10 +108,12 @@ Tracker::getPoint (double dx, double dy, double &x, double &y, double dist)
/// 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) const
+Tracker::computeError (double x, double y, double &el, double &ea, double &a)
+ const
{
- // Calcule la diférence avec la cible voulue.
+ // Calcule la différence avec la cible voulue.
double eX = x - posX_;
double eY = y - posY_;
// Calcule la distance.
@@ -120,6 +129,8 @@ Tracker::computeError (double x, double y, double &el, double &ea) const
// 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