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.cc40
1 files changed, 12 insertions, 28 deletions
diff --git a/2004/i/nono/src/motor/tracker.cc b/2004/i/nono/src/motor/tracker.cc
index e5779da..4ec28b9 100644
--- a/2004/i/nono/src/motor/tracker.cc
+++ b/2004/i/nono/src/motor/tracker.cc
@@ -81,44 +81,28 @@ Tracker::getAngleDiff (double a) const
return diff;
}
-/// Calcule l'erreur linéaire et angulaire pour atteindre un point,
-/// retourne faux si atteind.
+/// 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).
-/// 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
+void
+Tracker::computeError (double x, double y, double &el, double &ea) const
{
// Calcule la diférence avec la cible voulue.
double eX = x - posX_;
double eY = y - posY_;
- // Calcule la distance au carré.
- double d = eX * eX + eY * eY;
- // Vérifie l'eps.
- if (d < eps * eps)
- {
- el = 0;
- ea = 0;
- return false;
- }
- // Vérifie la distance max.
- if (d > dist * dist)
- {
- double ds = sqrt (d);
- eX = eX / ds * dist;
- eY = eY / ds * dist;
- }
+ // Calcule la distance.
+ double d = sqrt (eX * eX + eY * eY);
// Ramène sur -1..+1
- eX /= dist;
- eY /= dist;
+ 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 * cos (angle_) + eY * sin (angle_);
+ el = eX * ca + eY * sa;
// Calcule l'erreur angulaire (projection sur la direction
// perpendiculaire).
- ea = eX * -sin (angle_) + eY * cos (angle_);
- return true;
+ ea = eX * -sa + eY * ca;
}
/// Calcule l'erreur angulaire pour atteindre un angle, retourne faux si