From b776c0c39cc3160c190b8ae51640587030851de9 Mon Sep 17 00:00:00 2001 From: schodet Date: Sun, 7 Mar 2004 13:19:48 +0000 Subject: Nouveaux systeme de mouvement depuis la JPO. --- 2004/i/nono/src/motor/tracker.cc | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) (limited to '2004/i') diff --git a/2004/i/nono/src/motor/tracker.cc b/2004/i/nono/src/motor/tracker.cc index 7b77dea..ba90b95 100644 --- a/2004/i/nono/src/motor/tracker.cc +++ b/2004/i/nono/src/motor/tracker.cc @@ -77,18 +77,27 @@ Tracker::computeError (double x, double y, double &el, double &ea, double eps, double dist) const { // Calcule le point cible courant. - double aX = posX_ + dist * cos (angle_); - double aY = posY_ + dist * sin (angle_); + double aX = posX_; + double aY = posY_; // Calcule la diférence avec la cible voulue. double eX = x - aX; double eY = y - aY; + // Calcule la distance au carré. + double d = eX * eX + eY * eY; // Vérifie l'eps. - if (eX * eX + eY * eY < eps * 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 l'erreur linéaire (projection sur la direction courante). el = eX * cos (angle_) + eY * sin (angle_); // Calcule l'erreur angulaire (projection sur la direction -- cgit v1.2.3