From 3af9c88516543659002f5303d703f32579b45d6f Mon Sep 17 00:00:00 2001 From: schodet Date: Sun, 25 Apr 2004 20:48:52 +0000 Subject: Modification de Goto. --- 2004/i/nono/src/motor/Makefile.defs | 3 ++- 2004/i/nono/src/motor/goto.h | 17 ++++++++++------- 2004/i/nono/src/motor/goto_simple.cc | 23 ++++++++++------------- 2004/i/nono/src/motor/goto_simple.h | 14 ++++++++------ 2004/i/nono/src/motor/motor.cc | 8 ++++++++ 2004/i/nono/src/motor/motor.h | 3 +++ 2004/i/nono/src/motor/movement_goto.cc | 31 +++++++++++++++++++++---------- 2004/i/nono/src/motor/movement_goto.h | 3 +++ 2004/i/nono/src/motor/movement_types.h | 1 + 2004/i/nono/src/motor/test_motor.cc | 31 +++++++++++++++++++++++-------- 2004/i/nono/src/motor/tracker.cc | 33 +++++++++++++++++++++++++-------- 2004/i/nono/src/motor/tracker.h | 4 ++++ 12 files changed, 118 insertions(+), 53 deletions(-) (limited to '2004/i') diff --git a/2004/i/nono/src/motor/Makefile.defs b/2004/i/nono/src/motor/Makefile.defs index a273875..84ea47d 100644 --- a/2004/i/nono/src/motor/Makefile.defs +++ b/2004/i/nono/src/motor/Makefile.defs @@ -7,7 +7,8 @@ test_asserv_SOURCES = test_asserv.cc motor.a serial.a erreur.a config.a \ test_tracker_SOURCES = test_tracker.cc motor.a serial.a erreur.a config.a \ date.a utils.a logger.a motor_a_SOURCES = asserv.cc tracker.cc motor.cc movement_basic.cc \ - movement_goto.cc movement_rotation.cc goto_simple.cc + movement_goto.cc movement_rotation.cc goto_simple.cc \ + goto_circle.cc test_motor: $(test_motor_SOURCES:%.cc=%.o) diff --git a/2004/i/nono/src/motor/goto.h b/2004/i/nono/src/motor/goto.h index e062c5d..af6cedc 100644 --- a/2004/i/nono/src/motor/goto.h +++ b/2004/i/nono/src/motor/goto.h @@ -32,14 +32,17 @@ class Goto public: /// Destructeur. virtual ~Goto (void) { } - /// Fournit la distance au point final (mm) et renvois false si le Goto - /// est fini. - /// eps: distance (mm) en dessous de laquelle on considère que la + /// Initialise le Goto, appelé au début de la trajectoire. + virtual void init (const Tracker &t) { } + /// Fournit la distance au point final (mm), le point le plus loin à moins + /// de distmax (mm) et renvois false si le Goto est fini. + /// eps : distance (mm) en dessous de laquelle on considère que la /// destination est atteinte. - virtual bool getDistance (const Tracker &t, double &dist, double eps) = 0; - /// Fournit le point le plus loin à moins de dist (mm). - virtual void getPoint (const Tracker &t, double &dx, double &dy, - double dist) = 0; + /// distmax : distance (mm) maximale du point. + /// dist : distance (mm) au point final. + /// (x, y) : point intermédiaire (mm). + virtual bool get (const Tracker &t, double distmax, double eps, + double &dist, double &x, double &y) = 0; }; #endif // goto_h diff --git a/2004/i/nono/src/motor/goto_simple.cc b/2004/i/nono/src/motor/goto_simple.cc index fd6a16e..ffc23ce 100644 --- a/2004/i/nono/src/motor/goto_simple.cc +++ b/2004/i/nono/src/motor/goto_simple.cc @@ -31,22 +31,19 @@ GotoSimple::GotoSimple (double dx, double dy) { } -/// Fournit la distance au point final (mm) et renvois false si le Goto est -/// fini. -/// eps: distance (mm) en dessous de laquelle on considère que la -/// destination est atteinte. +/// Fournit la distance au point final (mm), le point le plus loin à moins +/// de distmax (mm) et renvois false si le Goto est fini. +/// eps : distance (mm) en dessous de laquelle on considère que la destination +/// est atteinte. +/// distmax : distance (mm) maximale du point. +/// dist : distance (mm) au point final. +/// (x, y) : point intermédiaire (mm). bool -GotoSimple::getDistance (const Tracker &t, double &dist, double eps) +GotoSimple::get (const Tracker &t, double distmax, double eps, double &dist, + double &x, double &y) { dist = t.getDistance (dx_, dy_); + t.getPoint (dx_, dy_, x, y, distmax); return dist > eps; } -/// Fournit le point le plus loin à moins de dist (mm). -void -GotoSimple::getPoint (const Tracker &t, double &dx, double &dy, double dist) -{ - dx = dx_; - dy = dy_; -} - diff --git a/2004/i/nono/src/motor/goto_simple.h b/2004/i/nono/src/motor/goto_simple.h index 950203e..d773720 100644 --- a/2004/i/nono/src/motor/goto_simple.h +++ b/2004/i/nono/src/motor/goto_simple.h @@ -34,13 +34,15 @@ class GotoSimple : public Goto /// Constructeur. /// (dx, dy) : position d'arrivée (mm). GotoSimple (double dx, double dy); - /// Fournit la distance au point final (mm) et renvois false si le Goto - /// est fini. - /// eps: distance (mm) en dessous de laquelle on considère que la + /// Fournit la distance au point final (mm), le point le plus loin à moins + /// de distmax (mm) et renvois false si le Goto est fini. + /// eps : distance (mm) en dessous de laquelle on considère que la /// destination est atteinte. - bool getDistance (const Tracker &t, double &dist, double eps); - /// Fournit le point le plus loin à moins de dist (mm). - void getPoint (const Tracker &t, double &dx, double &dy, double dist); + /// distmax : distance (mm) maximale du point. + /// dist : distance (mm) au point final. + /// (x, y) : point intermédiaire (mm). + bool get (const Tracker &t, double distmax, double eps, + double &dist, double &x, double &y); }; #endif // goto_simple_h diff --git a/2004/i/nono/src/motor/motor.cc b/2004/i/nono/src/motor/motor.cc index 7bbae46..45c9955 100644 --- a/2004/i/nono/src/motor/motor.cc +++ b/2004/i/nono/src/motor/motor.cc @@ -122,6 +122,14 @@ Motor::waitStopped (void) } while (!stopped ()); } +/// Paramètre la vitesse linéaire (-1..+1) et angulaire (-1..+1). Limite +/// la vitesse pour pouvoir freiner au bout de dist (mm). +void +Motor::speed (double l, double a, double dist) +{ + speed (computeSpeed (dist) * (l - a), computeSpeed (dist) * (l + a)); +} + /// Paramètre la vitesse des moteurs (-1..+1). void Motor::speed (double l, double r) diff --git a/2004/i/nono/src/motor/motor.h b/2004/i/nono/src/motor/motor.h index cd596a7..a5e15db 100644 --- a/2004/i/nono/src/motor/motor.h +++ b/2004/i/nono/src/motor/motor.h @@ -76,6 +76,9 @@ class Motor : public AsservTracker void waitStopped (void); /// Récupère l'object tracker. const Tracker &getTracker (void) const { return tracker_; } + /// Paramètre la vitesse linéaire (-1..+1) et angulaire (-1..+1). Limite + /// la vitesse pour pouvoir freiner au bout de dist (mm). + void speed (double l, double a, double dist); /// Paramètre la vitesse des moteurs (-1..+1). void speed (double l, double r); /// Calcule la vitesse en ligne droit afin de pouvoir freiner au bout diff --git a/2004/i/nono/src/motor/movement_goto.cc b/2004/i/nono/src/motor/movement_goto.cc index 6c5b198..cc720a9 100644 --- a/2004/i/nono/src/motor/movement_goto.cc +++ b/2004/i/nono/src/motor/movement_goto.cc @@ -68,24 +68,34 @@ MovementGoto::~MovementGoto (void) delete goto_; } +/// Initialise le mouvement, appelé juste quand le mouvement est mis en +/// service. +void +MovementGoto::init (const Tracker &t, Asserv &a, Motor &m) +{ + Movement::init (t, a, m); + goto_->init (t); +} + /// Controlle la vitesse, retourne faux si mouvement terminé. bool MovementGoto::control (void) { - // Récupère la distance. - double dist; - if (!goto_->getDistance (*t_, dist, param_.eps_)) + // Récupère la distance et le prochain point. + double dist, dx, dy; + if (!goto_->get (*t_, param_.dist_, param_.eps_, dist, dx, dy)) return false; - // Calcule la vitesse en ligne droite. - double speed = m_->computeSpeed (dist); - // Récupère le prochain point. - double dx, dy; - goto_->getPoint (*t_, dx, dy, param_.dist_); +std::cout << "movement goto: consign " << dist << ' ' << dx << ' ' << dy << + std::endl; // Calcule l'erreur. double el, ea; t_->computeError (dx, dy, el, ea); // Pas de marche arrière. - if (el < 0.0) el = 0.0; + if (el < 0.0) + { + el = 0.0; + ea = 2.0 - ea; + } std::cout << "movement goto: error " << el << ' ' << ea << std::endl; // Calcule les intégrales saturées. il_ += el; @@ -99,7 +109,8 @@ std::cout << "movement goto: error " << el << ' ' << ea << std::endl; + param_.kdl_ * (el - lel_)); double a = param_.kpa_ * (ea + param_.kia_ * ia_ + param_.kda_ * (ea - lea_)); - m_->speed (speed * (l - a), speed * (l + a)); +std::cout << "movement goto: command " << l << ' ' << a << std::endl; + m_->speed (l, a, dist); // Retiens l'erreur pour la dérivée. lel_ = el; lea_ = ea; diff --git a/2004/i/nono/src/motor/movement_goto.h b/2004/i/nono/src/motor/movement_goto.h index 95dde47..acbb3e1 100644 --- a/2004/i/nono/src/motor/movement_goto.h +++ b/2004/i/nono/src/motor/movement_goto.h @@ -67,6 +67,9 @@ class MovementGoto : public Movement MovementGoto (Goto *go); /// Destructeur. ~MovementGoto (void); + /// 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é. bool control (void); }; diff --git a/2004/i/nono/src/motor/movement_types.h b/2004/i/nono/src/motor/movement_types.h index 66ad1d0..7fb1ec4 100644 --- a/2004/i/nono/src/motor/movement_types.h +++ b/2004/i/nono/src/motor/movement_types.h @@ -28,5 +28,6 @@ #include "movement_goto.h" #include "movement_rotation.h" #include "goto_simple.h" +#include "goto_circle.h" #endif // movement_types_h diff --git a/2004/i/nono/src/motor/test_motor.cc b/2004/i/nono/src/motor/test_motor.cc index 27d4e68..42130bf 100644 --- a/2004/i/nono/src/motor/test_motor.cc +++ b/2004/i/nono/src/motor/test_motor.cc @@ -45,7 +45,8 @@ syntax (void) " b \n" " mouvement basique\n" " t va en (x, y)\n" - " r se dirige vers l'angle a en degrées\n" + " r se dirige vers l'angle a en degrés\n" + " c Suis un arc de rayon r et d'angle a en degrés\n" " attend un certain temps (qui augmente avec timeout)\n" " ? cet ecran d'aide" << std::endl; } @@ -62,17 +63,17 @@ main (int argc, char **argv) switch (argv[i][0]) { case 'g': - cout << "go" << endl; + cout << "test: go" << endl; i++; m.go (); break; case 'G': - cout << "ungo" << endl; + cout << "test: ungo" << endl; i++; m.go (false); break; case 'w': - cout << "wait stop" << endl; + cout << "test: wait stop" << endl; i++; m.waitStopped (); break; @@ -85,7 +86,7 @@ main (int argc, char **argv) speed = atof (argv[i++]); if (i >= argc) break; len = atof (argv[i++]); - cout << "basic " << speed << ' ' << len << endl; + cout << "test: basic " << speed << ' ' << len << endl; Movement *mov = new MovementBasic (speed, len); m.setMovement (mov); } @@ -98,7 +99,7 @@ main (int argc, char **argv) dX = atof (argv[i++]); if (i >= argc) break; dY = atof (argv[i++]); - cout << "goto " << dX << ' ' << dY << endl; + cout << "test: goto " << dX << ' ' << dY << endl; Goto *g = new GotoSimple (dX, dY); Movement *mov = new MovementGoto (g); m.setMovement (mov); @@ -110,12 +111,26 @@ main (int argc, char **argv) double dA; if (i >= argc) break; dA = atof (argv[i++]); - cout << "rotation " << dA << endl; + cout << "test: rotation " << dA << endl; Movement *mov = new MovementRotation (dA * 2 * M_PI / 360); m.setMovement (mov); } break; + case 'c': + { + i++; + double r, a; + if (i >= argc) break; + r = atof (argv[i++]); + if (i >= argc) break; + a = atof (argv[i++]); + cout << "test: circle " << r << ' ' << a << endl; + Goto *g = new GotoCircle (r, a * 2 * M_PI / 360); + Movement *mov = new MovementGoto (g); + m.setMovement (mov); + } + break; case '?': syntax (); return 0; @@ -127,7 +142,7 @@ main (int argc, char **argv) syntax (); return 1; } - cout << "sleep " << s << endl; + cout << "test: sleep " << s << endl; for (int j = 0; j < s * 1000 / 50; ++j) { m.ok (); 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 +#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. diff --git a/2004/i/nono/src/motor/tracker.h b/2004/i/nono/src/motor/tracker.h index 9648e3e..5e80a65 100644 --- a/2004/i/nono/src/motor/tracker.h +++ b/2004/i/nono/src/motor/tracker.h @@ -57,6 +57,10 @@ class Tracker double getAngle (void) const { return angle_; } /// Get distance. double getDistance (double x, double y) const; + /// Récupère les coordonnées d'un point à moins de dist (mm), dans la + /// direction de (dx, dy). + void getPoint (double dx, double dy, double &x, double &y, double dist) + const; /// Retourne vrai si arrété. bool stopped (void) { return zero_ > 10; } /// Récupère l'angle entre l'angle courant et a normalisé entre -pi et pi. -- cgit v1.2.3