summaryrefslogtreecommitdiff
path: root/2004/i/nono/src/motor
diff options
context:
space:
mode:
Diffstat (limited to '2004/i/nono/src/motor')
-rw-r--r--2004/i/nono/src/motor/asserv.cc12
-rw-r--r--2004/i/nono/src/motor/asserv.h2
-rw-r--r--2004/i/nono/src/motor/motor.cc20
-rw-r--r--2004/i/nono/src/motor/motor.h4
-rw-r--r--2004/i/nono/src/motor/movement.h10
-rw-r--r--2004/i/nono/src/motor/movement_basic.cc10
-rw-r--r--2004/i/nono/src/motor/movement_basic.h17
-rw-r--r--2004/i/nono/src/motor/movement_goto.cc5
-rw-r--r--2004/i/nono/src/motor/movement_goto.h16
-rw-r--r--2004/i/nono/src/motor/movement_rotation.cc84
-rw-r--r--2004/i/nono/src/motor/movement_rotation.h67
-rw-r--r--2004/i/nono/src/motor/test_asserv.cc25
-rw-r--r--2004/i/nono/src/motor/test_motor.cc27
-rw-r--r--2004/i/nono/src/motor/tracker.cc84
-rw-r--r--2004/i/nono/src/motor/tracker.h49
15 files changed, 354 insertions, 78 deletions
diff --git a/2004/i/nono/src/motor/asserv.cc b/2004/i/nono/src/motor/asserv.cc
index 2107908..e0ebd37 100644
--- a/2004/i/nono/src/motor/asserv.cc
+++ b/2004/i/nono/src/motor/asserv.cc
@@ -308,9 +308,11 @@ Asserv::handleStatMotor (void)
return;
}
char side = inBuf_[2];
- //int vacc = getSignedChar (inBuf_ + 2 + 1);
+ int vacc = getSignedChar (inBuf_ + 2 + 1);
int e = getSignedShort (inBuf_ + 2 + 1 + 2 + 1);
int pwm = getSignedShort (inBuf_ + 2 + 1 + 2 + 1 + 4 + 1);
+ log_ (Log::verydebug) << "stat motor " << side << ' ' << vacc << ' ' << e
+ << ' ' << pwm << std::endl;
}
/// Traite un message du compteur.
@@ -354,6 +356,14 @@ Asserv::handleCounter (void)
countRight_ = r;
}
+/// Décode un mot signé (1 octets).
+int
+Asserv::getSignedChar (const char *s) const
+{
+ return (signed char) (hex2digit (s[0]) << 4
+ | hex2digit (s[1]) << 0);
+}
+
/// Décode un mot signé (2 octets).
int
Asserv::getSignedShort (const char *s) const
diff --git a/2004/i/nono/src/motor/asserv.h b/2004/i/nono/src/motor/asserv.h
index cfda523..e95a3c7 100644
--- a/2004/i/nono/src/motor/asserv.h
+++ b/2004/i/nono/src/motor/asserv.h
@@ -106,6 +106,8 @@ class Asserv
void handleStatMotor (void);
/// Traite un message du compteur.
void handleCounter (void);
+ /// Décode un mot signé (1 octets).
+ int getSignedChar (const char *s) const;
/// Décode un mot signé (2 octets).
int getSignedShort (const char *s) const;
};
diff --git a/2004/i/nono/src/motor/motor.cc b/2004/i/nono/src/motor/motor.cc
index 9fc7c6c..f41dda8 100644
--- a/2004/i/nono/src/motor/motor.cc
+++ b/2004/i/nono/src/motor/motor.cc
@@ -25,7 +25,6 @@
#include "motor.h"
#include "config/config.h"
-#include <iostream>
#include <math.h>
// Pointeur vers l'instance unique.
@@ -33,7 +32,7 @@ Motor *Motor::instance_ = 0;
/// Constructeur.
Motor::Motor (void)
- : asserv_ (*this),
+ : asserv_ (*this), log_ ("motor"),
s_ (1.0), vs_ (1.0), as_ (1.0),
movement_ (0), maxSpeed_ (0x40), minSpeed_ (0x04)
{
@@ -121,20 +120,20 @@ Motor::waitStopped (void)
} while (!stopped ());
}
-/// Paramètre la vitesse des moteurs.
+/// Paramètre la vitesse des moteurs (-1..+1).
void
Motor::speed (double l, double r)
{
- int vL = (int) (l / vs_);
- int vR = (int) (r / vs_);
+ int vL = (int) ((vs_ > 0 ? l : -l) * maxSpeed_);
+ int vR = (int) ((vs_ > 0 ? r : -r) * maxSpeed_);
// Teste le dépassement de vitesse minimale.
while ((l != 0.0 || r != 0.0) && vL > -minSpeed_ && vL < minSpeed_ && vR >
-minSpeed_ && vR < minSpeed_)
{
l *= 1.5;
r *= 1.5;
- vL = (int) (l / vs_);
- vR = (int) (r / vs_);
+ vL = (int) ((vs_ > 0 ? l : -l) * maxSpeed_);
+ vR = (int) ((vs_ > 0 ? r : -r) * maxSpeed_);
}
// Teste le dépassement de vitesse maximal.
if (vL > maxSpeed_)
@@ -158,7 +157,8 @@ Motor::speed (double l, double r)
vL = (int) (l / r * vR);
}
// Envois la commande.
-std::cout << "motor speed " << l << ' ' << r << ' ' << vL << ' ' << vR << std::endl;;
+ log_ (Log::debug) << "speed " << l << ' ' << r << ' ' << vL << ' '
+ << vR << std::endl;;
asserv_.speed (vL, vR);
}
@@ -177,7 +177,7 @@ Motor::goToArc (double x, double y, double eps/*5.0*/)
double l, r;
if (tracker_.computeArcs (x, y, l, r, eps))
{
-std::cout << "goto arc " << l << ' ' << r;
+ log_ (Log::debug) << "goto arc " << l << ' ' << r;
if (l > 0.0)
l = sqrt (2.0 * asserv_.getAccel () * as_ * l);
else
@@ -186,7 +186,7 @@ std::cout << "goto arc " << l << ' ' << r;
r = sqrt (2.0 * asserv_.getAccel () * as_ * r);
else
r = -sqrt (2.0 * asserv_.getAccel () * as_ * -r);
-std::cout << ' ' << l << ' ' << r << std::endl;;
+ log_ << ' ' << l << ' ' << r << std::endl;
speed (l, r);
return true;
}
diff --git a/2004/i/nono/src/motor/motor.h b/2004/i/nono/src/motor/motor.h
index 7673534..fca17c4 100644
--- a/2004/i/nono/src/motor/motor.h
+++ b/2004/i/nono/src/motor/motor.h
@@ -37,6 +37,8 @@ class Motor : public AsservTracker
Asserv asserv_;
/// Suiveur de position.
Tracker tracker_;
+ // Système de log.
+ Log log_;
/// Pointeur vers l'instance unique.
static Motor *instance_;
/// Echelles (mm/unit, mm/s.unit, mm/s^2.unit).
@@ -72,7 +74,7 @@ class Motor : public AsservTracker
void waitStopped (void);
/// Récupère l'object tracker.
const Tracker &getTracker (void) const { return tracker_; }
- /// Paramètre la vitesse des moteurs.
+ /// Paramètre la vitesse des moteurs (-1..+1).
void speed (double l, double r);
/// Va vers un point, renvois faux si atteind.
bool goTo (double x, double y, double eps = 5.0);
diff --git a/2004/i/nono/src/motor/movement.h b/2004/i/nono/src/motor/movement.h
index 5a3deaf..e6f5e95 100644
--- a/2004/i/nono/src/motor/movement.h
+++ b/2004/i/nono/src/motor/movement.h
@@ -31,6 +31,8 @@ class Tracker;
class Asserv;
class Motor;
+/// Classe de base pour un mouvement. Classe gérant l'asservissement de la
+/// position.
class Movement
{
protected:
@@ -38,13 +40,13 @@ class Movement
Asserv *a_;
Motor *m_;
public:
- // Destructeur.
+ /// Destructeur.
virtual ~Movement (void) { }
- // Initialise le mouvement, appelé juste quand le mouvement est mis en
- // service.
+ /// Initialise le mouvement, appelé juste quand le mouvement est mis en
+ /// service.
virtual void init (const Tracker &t, Asserv &a, Motor &m)
{ t_ = &t; a_ = &a; m_ = &m; }
- // Controlle la vitesse, retourne faux si mouvement terminé.
+ /// Controlle la vitesse, retourne faux si mouvement terminé.
virtual bool control (void) { return false; }
};
diff --git a/2004/i/nono/src/motor/movement_basic.cc b/2004/i/nono/src/motor/movement_basic.cc
index 2b1cb6e..656ca35 100644
--- a/2004/i/nono/src/motor/movement_basic.cc
+++ b/2004/i/nono/src/motor/movement_basic.cc
@@ -27,15 +27,17 @@
#include "asserv.h"
#include "motor.h"
-// Constructeur.
+/// Constructeur.
+/// speed : vitesse (-1..+1).
+/// lenght : distance (mm, > 0)
MovementBasic::MovementBasic (double speed, double lenght)
: speed_ (speed), lenght2_ (lenght * lenght),
sX_ (0.0), sY_ (0.0)
{
}
-// Initialise le mouvement, appelé juste quand le mouvement est mis en
-// service.
+/// Initialise le mouvement, appelé juste quand le mouvement est mis en
+/// service.
void
MovementBasic::init (const Tracker &t, Asserv &a, Motor &m)
{
@@ -44,7 +46,7 @@ MovementBasic::init (const Tracker &t, Asserv &a, Motor &m)
sY_ = t.getY ();
}
-// Controlle la vitesse, retourne faux si mouvement terminé.
+/// Controlle la vitesse, retourne faux si mouvement terminé.
bool
MovementBasic::control (void)
{
diff --git a/2004/i/nono/src/motor/movement_basic.h b/2004/i/nono/src/motor/movement_basic.h
index 4c9a899..151d808 100644
--- a/2004/i/nono/src/motor/movement_basic.h
+++ b/2004/i/nono/src/motor/movement_basic.h
@@ -26,21 +26,24 @@
// }}}
#include "movement.h"
+/// Mouvemenet de base, avance tout droit jusqu'a avoir parcouru une distance.
class MovementBasic : public Movement
{
- // Vitesse.
+ /// Vitesse.
double speed_;
- // Distance au carré.
+ /// Distance au carré.
double lenght2_;
- // Position de départ.
+ /// Position de départ.
double sX_, sY_;
public:
- // Constructeur.
+ /// Constructeur.
+ /// speed : vitesse (-1..+1).
+ /// lenght : distance (mm, > 0)
MovementBasic (double speed, double lenght);
- // Initialise le mouvement, appelé juste quand le mouvement est mis en
- // service.
+ /// Initialise le mouvement, appelé juste quand le mouvement est mis en
+ /// service.
void init (const Tracker &t, Asserv &a, Motor &m);
- // Controlle la vitesse, retourne faux si mouvement terminé.
+ /// Controlle la vitesse, retourne faux si mouvement terminé.
bool control (void);
};
diff --git a/2004/i/nono/src/motor/movement_goto.cc b/2004/i/nono/src/motor/movement_goto.cc
index 794463c..b96526a 100644
--- a/2004/i/nono/src/motor/movement_goto.cc
+++ b/2004/i/nono/src/motor/movement_goto.cc
@@ -54,14 +54,15 @@ MovementGotoParam::MovementGotoParam (void)
}
}
-// Constructeur.
+/// Constructeur.
+/// (x, y) : position d'arrivée (mm).
MovementGoto::MovementGoto (double x, double y)
: dX_ (x), dY_ (y),
il_ (0.0), ia_ (0.0), lel_ (0.0), lea_ (0.0)
{
}
-// Controlle la vitesse, retourne faux si mouvement terminé.
+/// Controlle la vitesse, retourne faux si mouvement terminé.
bool
MovementGoto::control (void)
{
diff --git a/2004/i/nono/src/motor/movement_goto.h b/2004/i/nono/src/motor/movement_goto.h
index 721d481..375946f 100644
--- a/2004/i/nono/src/motor/movement_goto.h
+++ b/2004/i/nono/src/motor/movement_goto.h
@@ -26,6 +26,7 @@
// }}}
#include "movement.h"
+/// Paramètres d'un mouvement Goto.
class MovementGotoParam
{
// Epsilon.
@@ -46,20 +47,23 @@ class MovementGotoParam
friend class MovementGoto;
};
+/// Mouvement vers un point. Aucune garantie n'est fournie quand à la
+/// direction finale.
class MovementGoto : public Movement
{
- // Position d'arrivée.
+ /// Position d'arrivée.
double dX_, dY_;
- // Integrales.
+ /// Integrales.
double il_, ia_;
- // Dernière valeurs d'erreur.
+ /// Dernière valeurs d'erreur.
double lel_, lea_;
- // Paramètres d'asservissement.
+ /// Paramètres d'asservissement.
static MovementGotoParam param_;
public:
- // Constructeur.
+ /// Constructeur.
+ /// (x, y) : position d'arrivée (mm).
MovementGoto (double x, double y);
- // Controlle la vitesse, retourne faux si mouvement terminé.
+ /// Controlle la vitesse, retourne faux si mouvement terminé.
bool control (void);
};
diff --git a/2004/i/nono/src/motor/movement_rotation.cc b/2004/i/nono/src/motor/movement_rotation.cc
new file mode 100644
index 0000000..2f58e45
--- /dev/null
+++ b/2004/i/nono/src/motor/movement_rotation.cc
@@ -0,0 +1,84 @@
+// movement_rotation.cc
+// nono - programme du robot 2004. {{{
+//
+// Copyright (C) 2004 Nicolas Schodet
+//
+// Robot APB Team/Efrei 2004.
+// Web: http://assos.efrei.fr/robot/
+// Email: robot AT efrei DOT fr
+//
+// This program is free software; you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation; either version 2 of the License, or
+// (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+//
+// }}}
+#include "movement_rotation.h"
+#include "tracker.h"
+#include "motor.h"
+#include "config/config.h"
+
+MovementRotationParam MovementRotation::param_;
+
+/// Constructeur, charge les paramètres depuis la Config.
+MovementRotationParam::MovementRotationParam (void)
+ : eps_ (0.01),
+ kpa_ (1.0), kia_ (0.0), is_ (1000.0),
+ kda_ (0.0)
+{
+ // Lit la conf.
+ Config rc ("rc/movement/rotation");
+ while (!rc.eof ())
+ {
+ if (!(
+ rc.get ("epsilon", eps_) ||
+ rc.get ("kpa", kpa_) ||
+ rc.get ("kia", kia_) ||
+ rc.get ("is", is_) ||
+ rc.get ("kda", kda_)
+ ))
+ rc.noId ();
+ }
+}
+
+/// Constructeur.
+/// a : angle d'arrivé (rad).
+MovementRotation::MovementRotation (double a)
+ : dA_ (a),
+ ia_ (0.0), lea_ (0.0)
+{
+}
+
+/// Controlle la vitesse, retourne faux si mouvement terminé.
+bool
+MovementRotation::control (void)
+{
+ // Calcule l'erreur.
+ double ea;
+ if (!t_->computeAngleError (dA_, ea, param_.eps_))
+ {
+ return false;
+ }
+std::cout << "movement rotation error " << ea << std::endl;
+ // Calcule les intégrales saturées.
+ ia_ += ea;
+ if (ia_ > param_.is_) ia_ = param_.is_;
+ else if (ia_ < -param_.is_) ia_ = -param_.is_;
+ // Commande les moteurs.
+ double a = param_.kpa_ * (ea + param_.kia_ * ia_
+ + param_.kda_ * (ea - lea_));
+ m_->speed (a, -a);
+ // Retiens l'erreur pour la dérivée.
+ lea_ = ea;
+ return true;
+}
+
diff --git a/2004/i/nono/src/motor/movement_rotation.h b/2004/i/nono/src/motor/movement_rotation.h
new file mode 100644
index 0000000..4530ec3
--- /dev/null
+++ b/2004/i/nono/src/motor/movement_rotation.h
@@ -0,0 +1,67 @@
+#ifndef movement_rotation_h
+#define movement_rotation_h
+// movement_rotation.h
+// nono - programme du robot 2004. {{{
+//
+// Copyright (C) 2004 Nicolas Schodet
+//
+// Robot APB Team/Efrei 2004.
+// Web: http://assos.efrei.fr/robot/
+// Email: robot AT efrei DOT fr
+//
+// This program is free software; you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation; either version 2 of the License, or
+// (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+//
+// }}}
+#include "movement.h"
+
+/// Paramètres de MovementRotation.
+class MovementRotationParam
+{
+ // Epsilon.
+ double eps_;
+ // Coefficient proportionnel.
+ double kpa_;
+ // Coefficient intégral.
+ double kia_;
+ // Maximum de saturation de l'integralle.
+ double is_;
+ // Coefficient de dérivée.
+ double kda_;
+ public:
+ /// Constructeur, charge les paramètres depuis la Config.
+ MovementRotationParam (void);
+ friend class MovementRotation;
+};
+
+/// Mouvement de rotation, permet d'atteindre un angle.
+class MovementRotation : public Movement
+{
+ /// Angle d'arrivée.
+ double dA_;
+ /// Integrale.
+ double ia_;
+ /// Dernière valeurs d'erreur.
+ double lea_;
+ /// Paramètres d'asservissement.
+ static MovementRotationParam param_;
+ public:
+ /// Constructeur.
+ /// a : angle d'arrivé (rad).
+ MovementRotation (double a);
+ /// Controlle la vitesse, retourne faux si mouvement terminé.
+ bool control (void);
+};
+
+#endif // movement_rotation_h
diff --git a/2004/i/nono/src/motor/test_asserv.cc b/2004/i/nono/src/motor/test_asserv.cc
index 7ac096c..4aa05ae 100644
--- a/2004/i/nono/src/motor/test_asserv.cc
+++ b/2004/i/nono/src/motor/test_asserv.cc
@@ -36,6 +36,7 @@ class AsservTrackerC : public AsservTracker
{
int s;
public:
+ AsservTrackerC (void) : s (0) { }
// Appelée lors d'une mise à jour des compteurs.
virtual void updateCounter (int l, int r) { s += l; }
// Affiche.
@@ -46,6 +47,22 @@ class AsservTrackerC : public AsservTracker
{ s = 0; }
};
+void
+syntax (void)
+{
+ std::cout <<
+ "test_asserv - teste la communication avec la carte "
+ "d'asservissement.\n"
+ " s stop\n"
+ " z rezet\n"
+ " g go (active l'asservissement)\n"
+ " G ungo (désactive l'asservissement)\n"
+ " v <vg> <vd> vitesse gauche (vg) et droite (vd) en unité de la\n"
+ " carte d'asservissement\n"
+ " <timeout> attend un certain temps (qui augmente avec timeout)\n"
+ " ? cet ecran d'aide" << std::endl;
+}
+
int
main (int argc, char **argv)
{
@@ -89,9 +106,17 @@ main (int argc, char **argv)
cout << "speed " << vg << ' ' << vd << endl;
m.speed (vg, vd);
break;
+ case '?':
+ syntax ();
+ return 0;
default:
int s;
s = atoi (argv[i++]);
+ if (s == 0)
+ {
+ syntax ();
+ return 1;
+ }
cout << "sleep " << s << endl;
for (int j = 0; j < s * 1000 / 50; ++j)
{
diff --git a/2004/i/nono/src/motor/test_motor.cc b/2004/i/nono/src/motor/test_motor.cc
index 2f5f893..5ad15a2 100644
--- a/2004/i/nono/src/motor/test_motor.cc
+++ b/2004/i/nono/src/motor/test_motor.cc
@@ -34,6 +34,22 @@
using namespace std;
+void
+syntax (void)
+{
+ std::cout <<
+ "test_motor - teste la gestion du déplacement.\n"
+ " g go (active l'asservissement)\n"
+ " G ungo (désactive l'asservissement)\n"
+ " w wait stop (attend l'arret complet)\n"
+ " b <speed> <len>\n"
+ " mouvement basique\n"
+ " t <x> <y> va en (x, y)\n"
+ " r <a> se dirige vers l'angle a en degrées\n"
+ " <timeout> attend un certain temps (qui augmente avec timeout)\n"
+ " ? cet ecran d'aide" << std::endl;
+}
+
int
main (int argc, char **argv)
{
@@ -94,13 +110,22 @@ main (int argc, char **argv)
if (i >= argc) break;
dA = atof (argv[i++]);
cout << "rotation " << dA << endl;
- Movement *mov = new MovementRotation (dA);
+ Movement *mov = new MovementRotation (dA * 2 * M_PI /
+ 360);
m.setMovement (mov);
}
break;
+ case '?':
+ syntax ();
+ return 0;
default:
int s;
s = atoi (argv[i++]);
+ if (s == 0)
+ {
+ syntax ();
+ return 1;
+ }
cout << "sleep " << s << endl;
for (int j = 0; j < s * 1000 / 50; ++j)
{
diff --git a/2004/i/nono/src/motor/tracker.cc b/2004/i/nono/src/motor/tracker.cc
index 25f6f22..e5779da 100644
--- a/2004/i/nono/src/motor/tracker.cc
+++ b/2004/i/nono/src/motor/tracker.cc
@@ -25,13 +25,12 @@
#include "tracker.h"
#include "config/config.h"
-#include <iostream>
#include <math.h>
-// Constructeur.
+/// Constructeur.
Tracker::Tracker (void)
: posX_ (0.0), posY_ (0.0), angle_ (0.0),
- f_ (10.0), zero_ (0)
+ f_ (10.0), zero_ (0), log_ ("tracker")
{
// Lit la conf.
Config rc ("rc/tracker");
@@ -47,12 +46,12 @@ Tracker::Tracker (void)
}
}
-// Destructeur.
+/// Destructeur.
Tracker::~Tracker (void)
{
}
-// Set the position.
+/// Set the position.
void
Tracker::setPos (double x, double y, double angle)
{
@@ -61,7 +60,7 @@ Tracker::setPos (double x, double y, double angle)
angle_ = angle;
}
-// Get the position.
+/// Get the position.
void
Tracker::getPos (double &x, double &y, double &angle) const
{
@@ -70,8 +69,24 @@ Tracker::getPos (double &x, double &y, double &angle) const
angle = angle_;
}
-// Calcule l'erreur linéaire et angulaire pour atteindre un point,
-// retourne faux si atteind.
+/// Récupère l'angle entre l'angle courant et a normalisé entre -pi et pi.
+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;
+}
+
+/// Calcule l'erreur linéaire et angulaire pour atteindre un point,
+/// retourne faux si atteind.
+/// (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
@@ -95,6 +110,9 @@ Tracker::computeError (double x, double y, double &el, double &ea, double eps,
eX = eX / ds * dist;
eY = eY / ds * dist;
}
+ // Ramène sur -1..+1
+ eX /= dist;
+ eY /= 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
@@ -103,22 +121,27 @@ Tracker::computeError (double x, double y, double &el, double &ea, double eps,
return true;
}
-// Calcule l'erreur angulaire pour atteindre un angle, retourne faux si
-// atteind.
+/// Calcule l'erreur angulaire pour atteindre un angle, retourne faux si
+/// atteind.
+/// a : angle à atteindre (rad).
+/// ea : erreur angulaire (-1..+1).
+/// eps : angle en dessous duquel on considère qu'on est arrivé (rad).
bool
Tracker::computeAngleError (double a, double &ea, double eps) const
{
- ea = a - angle_;
- if (ea < eps)
+ ea = getAngleDiff (a);
+ if (-eps < ea && ea < eps)
{
ea = 0.0;
return false;
}
+ ea *= 1.0 / M_PI;
return true;
}
-// Calcule la longueur de l'arc gauche et droite pour atteindre un point.
-// EPS précise la distance minimale. Renvois faux si atteind.
+/// Calcule la longueur de l'arc gauche et droite pour atteindre un point.
+/// EPS précise la distance minimale. Renvois faux si atteind.
+/// \deprecated Pas vraiment l'algo qui faut.
bool
Tracker::computeArcs (double x, double y, double &l, double &r, double eps)
const
@@ -130,7 +153,7 @@ Tracker::computeArcs (double x, double y, double &l, double &r, double eps)
if (d < eps)
{
l = r = 0.0;
-std::cout << "compute arc eps" << std::endl;
+ log_ (Log::verydebug) << "compute arc eps" << std::endl;
return false;
}
// Calcule l'angle entre la direction courante et la direction du point à
@@ -143,24 +166,25 @@ std::cout << "compute arc eps" << std::endl;
if (dX * vY + dY * -vX < 0.0)
{
l = r = 0;
-std::cout << "compute arc back" << std::endl;
+ log_ (Log::verydebug) << "compute arc back" << std::endl;
return false;
}
// Calcul de l'angle par produit scalaire.
double s = dX * vX + dY * vY;
double dA = asin (s / d); // sin (a) = cos (a + pi/2)
-std::cout << "compute arc dA " << dA << " d " << d << std::endl;
+ log_ (Log::verydebug) << "compute arc dA " << dA << " d " << d <<
+ std::endl;
// Si l'angle est trops petit, on va tout droit, sinon /0.
if (dA > -0.0001 && dA < 0.0001)
{
l = r = d;
-std::cout << "compute arc small angle" << std::endl;
+ log_ (Log::verydebug) << "compute arc small angle" << std::endl;
}
// Si l'angle est trops grand, on ne fait pas le tour du monde.
else if (dA > M_PI_2 || dA < -M_PI_2)
{
l = r = 0;
-std::cout << "compute arc big angle" << std::endl;
+ log_ (Log::verydebug) << "compute arc big angle" << std::endl;
return false;
}
else
@@ -171,18 +195,19 @@ std::cout << "compute arc big angle" << std::endl;
if (ro < 0.25 * f_ && ro > 0.25 * f_)
{
l = r = 0.0;
-std::cout << "compute arc small ro " << ro << std::endl;
+ log_ (Log::verydebug) << "compute arc small ro " << ro <<
+ std::endl;
return false;
}
// L'angle du déplacement est le même que dA.
l = (ro - 0.5 * f_) * dA;
r = (ro + 0.5 * f_) * dA;
-std::cout << "compute arc ro " << ro << std::endl;
+ log_ (Log::verydebug) << "compute arc ro " << ro << std::endl;
}
return true;
}
-// Met à jour la position.
+/// Met à jour la position.
void
Tracker::update (double dL, double dR)
{
@@ -200,7 +225,8 @@ Tracker::update (double dL, double dR)
// Avec a petit (c'est le cas, car f_ >> abs (dR - dL)), a ~= atan (a).
double dA = (dR - dL) / f_;
double dS = 0.5 * (dL + dR);
-//std::cout << "tracker update dL " << dL << " dR " << dR << " dA " << dA << " dS " << dS << std::endl;
+ log_ (Log::verydebug) << "update dL " << dL << " dR " << dR << "
+ dA " << dA << " dS " << dS << std::endl;
// Si l'angle est petit, évite une division par presque 0.
if (dA < 0.0001 && dA > -0.0001)
{
@@ -210,7 +236,8 @@ Tracker::update (double dL, double dR)
posX_ += dS * cos (a);
posY_ += dS * sin (a);
angle_ += dA;
-//std::cout << "tracker small dX " << dS * cos (a) << " dY " << dS * sin (a) << std::endl;
+ log_ (Log::verydebug) << "small dX " << dS * cos (a) << " dY "
+ << dS * sin (a) << std::endl;
}
else
{
@@ -221,12 +248,15 @@ Tracker::update (double dL, double dR)
angle_ += dA;
posX_ += (sin (angle_) - sin (oldA)) * dS / dA;
posY_ += (cos (oldA) - cos (angle_)) * dS / dA;
-//std::cout << "tracker big dX " << -(sin (oldA) - sin (angle_)) * dS / dA << " dY " << -(cos (oldA) - cos (angle_)) * dS / dA << " R " << dS / dA << std::endl;
+ log_ (Log::verydebug) << "big dX " <<
+ -(sin (oldA) - sin (angle_)) * dS / dA <<
+ " dY " << -(cos (oldA) - cos (angle_)) * dS / dA <<
+ " R " << dS / dA << std::endl;
}
-std::cout << "update pos " << *this << std::endl;
+ log_ (Log::info) << "update pos " << *this << std::endl;
}
-// Affiche la position.
+/// Affiche la position.
std::ostream &
operator<< (std::ostream &os, const Tracker &t)
{
diff --git a/2004/i/nono/src/motor/tracker.h b/2004/i/nono/src/motor/tracker.h
index 2674782..63c58b2 100644
--- a/2004/i/nono/src/motor/tracker.h
+++ b/2004/i/nono/src/motor/tracker.h
@@ -24,46 +24,65 @@
// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
//
// }}}
+#include "logger/log.h"
#include <iostream>
+/// Classe de suivi des coordonnée et de calcul d'erreur.
class Tracker
{
- // Position et angle (mm et rad).
+ /// Position et angle (mm et rad).
double posX_, posY_, angle_;
- // Distance entre les deux roues (mm).
+ /// Distance entre les deux roues (mm).
double f_;
- // Nombre d'updates nuls consécutifs.
+ /// Nombre d'updates nuls consécutifs.
int zero_;
+ // Système de log.
+ mutable Log log_;
public:
- // Constructeur.
+ /// Constructeur.
Tracker (void);
- // Destructeur.
+ /// Destructeur.
~Tracker (void);
- // Set the position.
+ /// Set the position.
void setPos (double x, double y, double angle);
- // Get the position.
+ /// Get the position.
void getPos (double &x, double &y, double &angle) const;
+ /// Get X position.
double getX (void) const { return posX_; }
+ /// Get Y position.
double getY (void) const { return posY_; }
+ /// Get angle.
double getAngle (void) const { return angle_; }
+ /// Retourne vrai si arrété.
bool stopped (void) { return zero_ > 10; }
- // Calcule l'erreur linéaire et angulaire pour atteindre un point,
- // retourne faux si atteind.
+ /// Récupère l'angle entre l'angle courant et a normalisé entre -pi et pi.
+ double getAngleDiff (double a) const;
+ /// Calcule l'erreur linéaire et angulaire pour atteindre un point,
+ /// retourne faux si atteind.
+ /// (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 computeError (double x, double y, double &el, double &ea, double eps,
double dist) const;
- // Calcule l'erreur angulaire pour atteindre un angle, retourne faux si
- // atteind.
+ /// Calcule l'erreur angulaire pour atteindre un angle, retourne faux si
+ /// atteind.
+ /// a : angle à atteindre (rad).
+ /// ea : erreur angulaire (-1..+1).
+ /// eps : angle en dessous duquel on considère qu'on est arrivé (rad).
bool computeAngleError (double a, double &ea, double eps) const;
- // Calcule la longueur de l'arc gauche et droite pour atteindre un point.
- // EPS précise la distance minimale. Renvois faux si atteind.
+ /// Calcule la longueur de l'arc gauche et droite pour atteindre un point.
+ /// EPS précise la distance minimale. Renvois faux si atteind.
+ /// \deprecated Pas vraiment l'algo qui faut.
bool computeArcs (double x, double y, double &l, double &r, double eps)
const;
- // Met à jour la position.
+ /// Met à jour la position.
void update (double dL, double dR);
};
-// Affiche la position.
+/// Affiche la position.
std::ostream &operator<< (std::ostream &os, const Tracker &t);
#endif // tracker_h