summaryrefslogtreecommitdiff
path: root/2004/i/nono
diff options
context:
space:
mode:
authorschodet2004-03-07 13:19:48 +0000
committerschodet2004-03-07 13:19:48 +0000
commitb776c0c39cc3160c190b8ae51640587030851de9 (patch)
tree2694c5cc5bc143c7667d72597dd20bfd3aeeee93 /2004/i/nono
parentf70c1bc36ac9820ff9a4eb2d78c4196cfbb79557 (diff)
Nouveaux systeme de mouvement depuis la JPO.
Diffstat (limited to '2004/i/nono')
-rw-r--r--2004/i/nono/src/motor/tracker.cc15
1 files changed, 12 insertions, 3 deletions
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