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.cc33
1 files changed, 25 insertions, 8 deletions
diff --git a/2004/i/nono/src/motor/tracker.cc b/2004/i/nono/src/motor/tracker.cc
index 4ec28b9..270455a 100644
--- a/2004/i/nono/src/motor/tracker.cc
+++ b/2004/i/nono/src/motor/tracker.cc
@@ -24,8 +24,7 @@
// }}}
#include "tracker.h"
#include "config/config.h"
-
-#include <math.h>
+#include "utils/mathutil.h"
/// Constructeur.
Tracker::Tracker (void)
@@ -73,12 +72,30 @@ Tracker::getPos (double &x, double &y, double &angle) const
double
Tracker::getAngleDiff (double a) const
{
- double diff = angle_ - a;
- while (diff > M_PI)
- diff -= M_PI;
- while (diff < -M_PI)
- diff += M_PI;
- return diff;
+ return angleNorm (angle_ - a);
+}
+
+/// Récupère les coordonnées d'un point à moins de dist (mm), dans la
+/// direction de (dx, dy).
+void
+Tracker::getPoint (double dx, double dy, double &x, double &y, double dist)
+ const
+{
+ // Calcule le delta.
+ double dtx = dx - posX_;
+ double dty = dy - posY_;
+ // Test si trops loin.
+ double sqd = dtx * dtx + dty * dty;
+ if (sqd > dist * dist)
+ {
+ x = posX_ + dtx * dist / sqrt (sqd);
+ y = posY_ + dty * dist / sqrt (sqd);
+ }
+ else
+ {
+ x = dx;
+ y = dy;
+ }
}
/// Calcule l'erreur linéaire et angulaire pour atteindre un point.