From fbf9fdbeb40baf2431da481d14edfa1d232dc184 Mon Sep 17 00:00:00 2001 From: schodet Date: Sun, 9 May 2004 18:03:51 +0000 Subject: gpio hermite limitation de la vitesse angulaire movement queue motor_cmd --- 2004/i/nono/src/motor/Makefile.defs | 6 +- 2004/i/nono/src/motor/asserv.cc | 49 +++++++++- 2004/i/nono/src/motor/asserv.h | 11 +++ 2004/i/nono/src/motor/goto_hermite.cc | 82 +++++++++++----- 2004/i/nono/src/motor/goto_hermite.h | 18 +++- 2004/i/nono/src/motor/motor.cc | 121 +++++++++++++++++------- 2004/i/nono/src/motor/motor.h | 28 ++++-- 2004/i/nono/src/motor/motor_cmd.cc | 147 +++++++++++++++++++++++++++++ 2004/i/nono/src/motor/motor_cmd.h | 35 +++++++ 2004/i/nono/src/motor/movement_goto.cc | 8 +- 2004/i/nono/src/motor/movement_types.h | 1 + 2004/i/nono/src/motor/test_goto_hermite.cc | 7 ++ 2004/i/nono/src/motor/test_motor.cc | 123 +++++------------------- 2004/i/nono/src/motor/tracker.cc | 15 ++- 2004/i/nono/src/motor/tracker.h | 7 +- 15 files changed, 483 insertions(+), 175 deletions(-) create mode 100644 2004/i/nono/src/motor/motor_cmd.cc create mode 100644 2004/i/nono/src/motor/motor_cmd.h (limited to '2004/i') diff --git a/2004/i/nono/src/motor/Makefile.defs b/2004/i/nono/src/motor/Makefile.defs index 033783f..2453e0d 100644 --- a/2004/i/nono/src/motor/Makefile.defs +++ b/2004/i/nono/src/motor/Makefile.defs @@ -1,7 +1,7 @@ TARGETS += test_motor test_asserv test_tracker test_goto_hermite LIBS += motor.a -test_motor_SOURCES = test_motor.cc motor.a serial.a erreur.a config.a date.a \ - utils.a logger.a +test_motor_SOURCES = test_motor.cc motor_cmd.cc motor.a serial.a erreur.a \ + config.a date.a utils.a logger.a test_asserv_SOURCES = test_asserv.cc motor.a serial.a erreur.a config.a \ date.a utils.a logger.a test_tracker_SOURCES = test_tracker.cc motor.a serial.a erreur.a config.a \ @@ -9,7 +9,7 @@ test_tracker_SOURCES = test_tracker.cc motor.a serial.a erreur.a config.a \ motor_a_SOURCES = asserv.cc tracker.cc motor.cc \ movement_basic.cc movement_goto.cc movement_rotation.cc \ movement_round_cruise.cc \ - goto_simple.cc goto_circle.cc + goto_simple.cc goto_circle.cc goto_hermite.cc test_goto_hermite_SOURCES = goto_hermite.cc test_goto_hermite.cc tracker.cc \ config.a logger.a erreur.a diff --git a/2004/i/nono/src/motor/asserv.cc b/2004/i/nono/src/motor/asserv.cc index c806479..7be14f7 100644 --- a/2004/i/nono/src/motor/asserv.cc +++ b/2004/i/nono/src/motor/asserv.cc @@ -30,12 +30,14 @@ /// Constructeur. Asserv::Asserv (AsservTracker &asservTracker) : ttyspeed_ (0), accel_ (-1), kp_ (-1), ki_ (-1), kd_ (-1), - statMotor_ (-1), counter_ (false), posAsserv_ (false), asserv_ (false), + statMotor_ (-1), counter_ (false), posAsserv_ (false), gpiDelay_ (0), + asserv_ (false), zeroEps_ (0), noSetParam_ (false), inBufSize_ (64), inBufPos_ (0), inBuf_ (0), firstCounter_ (true), countLeft_ (0), countRight_ (0), + gpi_ (0), asservTracker_ (asservTracker), log_ ("asserv") { @@ -53,13 +55,14 @@ Asserv::Asserv (AsservTracker &asservTracker) rc.get ("stats", statMotor_) || rc.get ("counter", counter_) || rc.get ("posasserv", posAsserv_) || + rc.get ("gpi", gpiDelay_) || rc.get ("zeroeps", zeroEps_) || rc.get ("buffer", inBufSize_) )) rc.noId (); } // Ouvre le port série. - serial_.open (ttyname_.c_str (), ttyspeed_); + serial_.open (ttyname_, ttyspeed_); // Alloue la mémoire du tampon d'entrée. inBuf_ = new char[inBufSize_ + 2]; } @@ -105,6 +108,8 @@ Asserv::setParam (void) setStatMotor (statMotor_); if (posAsserv_) setPosAsserv (posAsserv_); + if (gpiDelay_) + setGpiDelay (gpiDelay_); if (asserv_) setAsserv (asserv_); if (counter_) @@ -216,6 +221,13 @@ Asserv::setPosAsserv (bool fl/*true*/) posAsserv_ = fl; } +void +Asserv::setGpiDelay (int delay) +{ + send ('h', delay); + gpiDelay_ = delay; +} + void Asserv::setAsserv (bool fl/*true*/) { @@ -237,6 +249,13 @@ Asserv::getAccelFactor (void) const return getSpeedFactor () / 0.004 / accel_; } +/// Envois la sortie GPO. +void +Asserv::sendGpo (unsigned int gpo) +{ + send ('k', static_cast (gpo)); +} + /// Envoie un message. void Asserv::send (char com) @@ -335,6 +354,10 @@ Asserv::handleMessage (void) // Recois une nouvelle valeur pour les compteurs. handleCounter (); break; + case 'H': + // Recois un GPI. + handleGpi (); + break; case 'T': // TTL expiré, log un message. log_ (Log::warning) << "ttl expired" << std::endl; @@ -449,3 +472,25 @@ Asserv::handleCounter (void) countLeft_ = l; countRight_ = r; } + +/// Traite les GPI. +void +Asserv::handleGpi (void) +{ + if (inBufPos_ != 2 + 2 + 1) + { + // Mauvaise transmission. + log_ (Log::warning) << "gpi error (n)" << std::endl; + return; + } + unsigned int gpi = hexUnsignedChar2int (inBuf_ + 2); + if (gpi == static_cast (hexInvalid)) + { + // Mauvaise transmission. + log_ (Log::warning) << "gpi error (?)" << std::endl; + return; + } + // Recopie la valeur. + gpi_ = gpi; +} + diff --git a/2004/i/nono/src/motor/asserv.h b/2004/i/nono/src/motor/asserv.h index c391e41..aa39324 100644 --- a/2004/i/nono/src/motor/asserv.h +++ b/2004/i/nono/src/motor/asserv.h @@ -53,6 +53,7 @@ class Asserv int statMotor_; bool counter_; bool posAsserv_; + int gpiDelay_; bool asserv_; int zeroEps_; bool noSetParam_; @@ -64,6 +65,8 @@ class Asserv // Anciènne valeur des compteur. bool firstCounter_; int countLeft_, countRight_; + // Valeur de l'entrée GPI. + unsigned int gpi_; // Objet interessé par les stats. AsservTracker &asservTracker_; // Système de log. @@ -96,12 +99,18 @@ class Asserv void setStatMotor (int delay); void setCounter (bool fl = true); void setPosAsserv (bool fl = true); + void setGpiDelay (int delay); void setAsserv (bool fl = true); //@} /// Get factor to deduce speed scale from scale. double getSpeedFactor (void) const; /// Get factor to deduce accel scale from scale. double getAccelFactor (void) const; + /// Récupère l'entrée GPI. + unsigned int getGpi (void) const + { return gpi_; } + /// Envois la sortie GPO. + void sendGpo (unsigned int gpo); protected: /// @{ /// Envoie un message. @@ -120,6 +129,8 @@ class Asserv void handleStatMotor (void); /// Traite un message du compteur. void handleCounter (void); + /// Traite les GPI. + void handleGpi (void); }; #endif // asserv_h diff --git a/2004/i/nono/src/motor/goto_hermite.cc b/2004/i/nono/src/motor/goto_hermite.cc index a1fc6c5..9e47296 100644 --- a/2004/i/nono/src/motor/goto_hermite.cc +++ b/2004/i/nono/src/motor/goto_hermite.cc @@ -29,7 +29,18 @@ /// Constructeur. /// fa : angle final. GotoHermite::GotoHermite (double fa) - : step_ (0.0), ia_ (0.0), fa_ (fa) + : step_ (0.0), lastDist_ (0.0), ia_ (0.0), fa_ (fa), + di_ (defaultDi_), df_ (defaultDf_) +{ + points_.push_back (Point ()); +} + +/// Constructeur. +/// fa : angle final. +/// di : longueur de la tangente initiale. +/// df : longueur de la tangente finale. +GotoHermite::GotoHermite (double fa, double di, double df) + : step_ (0.0), lastDist_ (0.0), ia_ (0.0), fa_ (fa), di_ (di), df_ (df) { points_.push_back (Point ()); } @@ -38,7 +49,19 @@ GotoHermite::GotoHermite (double fa) void GotoHermite::add (double x, double y) { - points_.push_back (Point (x, y)); + Point newPoint (x, y); + // Met à jour le tableau des distances. + double dist = newPoint.distTo (points_.back ()); + for (Dists::iterator i = dists_.begin (); + i != dists_.end (); + ++i) + { + *i += dist; + } + dists_.push_back (0.0); + lastDist_ += dist; + // Ajoute le point. + points_.push_back (newPoint); } /// Initialise le Goto, appelé au début de la trajectoire. @@ -47,7 +70,7 @@ GotoHermite::init (const Tracker &t) { double x, y, a; t.getPos (x, y, a); - points_[0] = Point (x, y); + lastPoint_ = points_[0] = Point (x, y); ia_ = a; } @@ -62,7 +85,22 @@ bool GotoHermite::get (const Tracker &t, double distmax, double eps, double &dist, double &x, double &y) { - return false; + Point cur (t.getX (), t.getY ()); + // Trouve le point intermédiaire. + double sqDistMax = distmax * distmax; + while (cur.sqDistTo (lastPoint_) < sqDistMax) + { + step_ += stepSize_; + if (!computePoint (step_, lastPoint_, lastDist_)) + break; + } + // Trouve la distance au point final. + dist = lastDist_ + cur.distTo (lastPoint_); + if (eps > dist) + return false; + x = lastPoint_.x; + y = lastPoint_.y; + return true; } /// Test le GotoHermite en affichant la liste des points générés. @@ -70,17 +108,19 @@ void GotoHermite::test (std::ostream &os) const { Point p; + double dist; for (double s = 0.0; ; s += stepSize_) { - if (!computePoint (s, p)) + if (!computePoint (s, p, dist)) break; - std::cout << p << std::endl; + std::cout << p << ' ' << dist << std::endl; } } -/// Calcule le point au pas step, renvois faux si fini. +/// Calcule le point au pas step et la distance restant à parcourir, +/// renvois faux si fini. bool -GotoHermite::computePoint (double step, Point &p) const +GotoHermite::computePoint (double step, Point &p, double &dist) const { // Vérifications d'overflow. if (step < 0.0) @@ -108,12 +148,9 @@ GotoHermite::computePoint (double step, Point &p) const } else { - // Projette le vecteur vers le point suivant sur le vecteur angle. t1 = p1; - double cia = cos (ia_); - double sia = sin (ia_); - t1.x += cia * (p2.x - p1.x) + sia * (p2.y - p1.y); - t1.x += -sia * (p2.x - p1.x) + cia * (p2.y - p1.y); + t1.x = cos (ia_) * di_; + t1.y = sin (ia_) * di_; } Point t2; if (i < static_cast (points_.size ()) - 2) @@ -123,18 +160,19 @@ GotoHermite::computePoint (double step, Point &p) const } else { - // Projette le vecteur depuis le point précédent sur le vecteur angle. t2 = p2; - double cia = cos (fa_); - double sia = sin (fa_); - t2.x += cia * (p2.x - p1.x) + sia * (p2.y - p1.y); - t2.x += -sia * (p2.x - p1.x) + cia * (p2.y - p1.y); + t2.x = cos (fa_) * df_; + t2.y = sin (fa_) * df_; } - // Aplique Hermite. + // Applique Hermite. + t1 *= h3; + t2 *= h4; p = p1 * h1; - p+= t1 * h3; - p+= t2 * h4; - p+= p2 * h2; + p += t1; + p += t2; + p += p2 * h2; + // Calcule la distance au point final. + dist = dists_[i] + p.distTo (points_[i + 1]); return true; } diff --git a/2004/i/nono/src/motor/goto_hermite.h b/2004/i/nono/src/motor/goto_hermite.h index bc6fb7e..8815e12 100644 --- a/2004/i/nono/src/motor/goto_hermite.h +++ b/2004/i/nono/src/motor/goto_hermite.h @@ -36,14 +36,25 @@ class GotoHermite : public Goto { typedef std::vector Points; Points points_; + typedef std::vector Dists; + Dists dists_; double step_; + Point lastPoint_; + double lastDist_; static const double stepSize_ = 0.01; static const double tightness_ = 0.5; - double ia_, fa_; + static const double defaultDi_ = 0.0; + static const double defaultDf_ = 600.0; + double ia_, fa_, di_, df_; public: /// Constructeur. /// fa : angle final. GotoHermite (double fa); + /// Constructeur. + /// fa : angle final. + /// di : longueur de la tangente initiale. + /// df : longueur de la tangente finale. + GotoHermite (double fa, double di, double df); /// Ajoute un point au chemin. void add (double x, double y); /// Initialise le Goto, appelé au début de la trajectoire. @@ -60,8 +71,9 @@ class GotoHermite : public Goto /// Test le GotoHermite en affichant la liste des points générés. void test (std::ostream &os) const; protected: - /// Calcule le point au pas step, renvois faux si fini. - bool computePoint (double step, Point &p) const; + /// Calcule le point au pas step et la distance restant à parcourir, + /// renvois faux si fini. + bool computePoint (double step, Point &p, double &dist) const; }; #endif // goto_hermite_h diff --git a/2004/i/nono/src/motor/motor.cc b/2004/i/nono/src/motor/motor.cc index ddd00ce..902e46f 100644 --- a/2004/i/nono/src/motor/motor.cc +++ b/2004/i/nono/src/motor/motor.cc @@ -33,7 +33,7 @@ Motor *Motor::instance_ = 0; /// Constructeur. Motor::Motor (void) : asserv_ (*this), log_ ("motor"), - s_ (1.0), vs_ (1.0), a_ (1.0), + s_ (1.0), vs_ (1.0), a_ (1.0), fact_ (0.5), brakeDistance_ (0.0), movement_ (0), maxSpeed_ (0x40), minSpeed_ (0x04) { @@ -45,6 +45,7 @@ Motor::Motor (void) { if (!( rc.get ("scale", s_) || + rc.get ("fact", fact_) || rc.get ("maxspeed", maxSpeed_) || rc.get ("minspeed", minSpeed_) )) @@ -63,8 +64,8 @@ Motor::Motor (void) /// Destructeur. Motor::~Motor (void) { + clearMovement (); instance_ = 0; - delete movement_; } /// Appelée lors d'une mise à jour des compteurs. @@ -76,22 +77,43 @@ Motor::updateCounter (int l, int r, bool zero) double dL = l * s_; double dR = r * s_; tracker_.update (dL, dR, zero); + if (!movement_) + { + if (!nextMovement ()) + return; + } if (movement_) - if (!movement_->control ()) + { + while (!movement_->control ()) { - delete movement_; - movement_ = 0; - asserv_.stop (); + if (!nextMovement ()) + { + // C'est la fin. + asserv_.stop (); + return; + } } + } + + +} + +/// Ajoute un mouvement à la file d'attente. +void +Motor::addMovement (Movement *m) +{ + movementQueue_.push_back (m); } -/// Choisi le mouvement en cours. +/// Vide la file d'attente. void -Motor::setMovement (Movement *m) +Motor::clearMovement (void) { delete movement_; - movement_ = m; - m->init (tracker_, asserv_, *this); + MovementQueue::iterator first = movementQueue_.begin (); + MovementQueue::iterator last = movementQueue_.end (); + for ( ; first != last; ++first) + delete *first; } /// Teste si les mouvements sont terminés. @@ -99,7 +121,7 @@ bool Motor::end (void) { ok (); - return !movement_; + return !movement_ && movementQueue_.empty (); } /// Attend la fin de tout les mouvements. @@ -129,64 +151,83 @@ Motor::waitStopped (void) } /// Paramètre la vitesse linéaire (-1..+1) et angulaire (-1..+1). Limite -/// la vitesse pour pouvoir freiner au bout de dist (mm). +/// la vitesse pour pouvoir freiner au bout de dist (mm) et au bout de +/// l'angle (rad). void -Motor::speed (double l, double a, double dist) +Motor::speed (double l, double a, double dist, double angle) { - speedLimit (l - a, l + a, computeSpeed (dist)); + double liml, lima; + liml = computeSpeed (dist); + lima = computeSpeed (tracker_.getAngleDistance (angle)); + speedLimit (l - a, l + a, liml, lima); } /// Paramètre la vitesse des moteurs (-1..+1). void Motor::speed (double l, double r) { - speedLimit (l, r, maxSpeed_); + speedLimit (l, r, maxSpeed_, 2 * maxSpeed_); } /// Paramètre la vitesse des moteurs (-1..+1) avec une vitesse limite -/// (unit). +/// linéaire en angulaire (unit). void -Motor::speedLimit (double l, double r, double vLimit) +Motor::speedLimit (double l, double r, double vLimitL, double vLimitA) { // Il faut un minimum tout de même. La politique, c'est on avance. - if (vLimit < minSpeed_) - vLimit = minSpeed_; + if (vLimitL < minSpeed_) + vLimitL = minSpeed_; + if (vLimitA < minSpeed_) + vLimitA = minSpeed_; // Convertie le (-1..+1) en (unit). - int vL = (int) ((vs_ > 0 ? l : -l) * maxSpeed_ * 0.5); - int vR = (int) ((vs_ > 0 ? r : -r) * maxSpeed_ * 0.5); + int vL = (int) ((vs_ > 0 ? l : -l) * maxSpeed_ * fact_); + int vR = (int) ((vs_ > 0 ? r : -r) * maxSpeed_ * fact_); // 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) ((vs_ > 0 ? l : -l) * maxSpeed_ * 0.5); - vR = (int) ((vs_ > 0 ? r : -r) * maxSpeed_ * 0.5); + vL = (int) ((vs_ > 0 ? l : -l) * maxSpeed_ * fact_); + vR = (int) ((vs_ > 0 ? r : -r) * maxSpeed_ * fact_); } - // Teste le dépassement de vitesse maximal. - if (vL > vLimit) + // Teste le dépassement de vitesse maximale linéaire. + if (vL > vLimitL) { - vL = static_cast (vLimit); + vL = static_cast (vLimitL); vR = (int) (r / l * vL); } - else if (vL < -vLimit) + else if (vL < -vLimitL) { - vL = static_cast (-vLimit); + vL = static_cast (-vLimitL); vR = (int) (r / l * vL); } - if (vR > vLimit) + if (vR > vLimitL) { - vR = static_cast (vLimit); + vR = static_cast (vLimitL); vL = (int) (l / r * vR); } - else if (vR < -vLimit) + else if (vR < -vLimitL) { - vR = static_cast (-vLimit); + vR = static_cast (-vLimitL); vL = (int) (l / r * vR); } + // Teste le dépassement de vitesse maximale angulaire. + if (vL - vR > vLimitA) + { + double fact = vLimitA / (vL - vR); + vR = static_cast (vR * fact); + vL = static_cast (vL * fact); + } + else if (vR - vL > vLimitA) + { + double fact = vLimitA / (vR - vL); + vR = static_cast (vR * fact); + vL = static_cast (vL * fact); + } // Envois la commande. log_ (Log::debug) << "speed " << l << ' ' << r << ' ' << vL << ' ' - << vR << ' ' << vLimit << std::endl;; + << vR << ' ' << vLimitL << ' ' << vLimitA << std::endl;; asserv_.speed (vL, vR); } @@ -231,3 +272,17 @@ Motor::goToArc (double x, double y, double eps/*5.0*/) } } +/// Passe au mouvement suivant. +bool +Motor::nextMovement (void) +{ + delete movement_; + movement_ = 0; + if (movementQueue_.empty ()) + return false; + movement_ = movementQueue_.front (); + movementQueue_.pop_front (); + movement_->init (tracker_, asserv_, *this); + return true; +} + diff --git a/2004/i/nono/src/motor/motor.h b/2004/i/nono/src/motor/motor.h index e7312a9..b33ede3 100644 --- a/2004/i/nono/src/motor/motor.h +++ b/2004/i/nono/src/motor/motor.h @@ -28,6 +28,8 @@ #include "tracker.h" #include "movement.h" +#include + /// Gestion des moteurs principaux du robot. Cette classe gére les /// déplacements simples (enfin pas si simple que ça) comme aller vers un /// point, atteindre un angle... @@ -43,9 +45,13 @@ class Motor : public AsservTracker static Motor *instance_; /// Echelles (mm/unit, mm/s.unit, mm/s^2.unit). double s_, vs_, a_; + /// Echelles entre l'unitée et la vitesse max. + double fact_; /// Distance de freinage à vitesse maximale. double brakeDistance_; - /// Mouvement en cours. + /// File de mouvements. + typedef std::list MovementQueue; + MovementQueue movementQueue_; Movement *movement_; /// Vitesses maximale et minimale (unit). int maxSpeed_, minSpeed_; @@ -60,8 +66,10 @@ class Motor : public AsservTracker /// l, r : mise à jour moteur gauche et droit (unit). /// zero : vrai si le mouvement est à considérer comme nul. void updateCounter (int l, int r, bool zero); - /// Choisi le mouvement en cours. - void setMovement (Movement *m); + /// Ajoute un mouvement à la file d'attente. + void addMovement (Movement *m); + /// Vide la file d'attente. + void clearMovement (void); /// Active l'asservissement. void go (bool fl = true) { asserv_.setAsserv (fl); } /// Teste si l'émission est terminée. @@ -78,20 +86,26 @@ class Motor : public AsservTracker void waitStopped (void); /// Récupère l'object tracker. const Tracker &getTracker (void) const { return tracker_; } + /// Récupère l'object asserv. + Asserv &getAsserv (void) { return asserv_; } /// 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); + /// la vitesse pour pouvoir freiner au bout de dist (mm) et au bout de + /// l'angle (rad). + void speed (double l, double a, double dist, double angle); /// Paramètre la vitesse des moteurs (-1..+1). void speed (double l, double r); /// Paramètre la vitesse des moteurs (-1..+1) avec une vitesse limite - /// (unit). - void speedLimit (double l, double r, double vLimit); + /// linéaire en angulaire (unit). + void speedLimit (double l, double r, double vLimitL, double vLimitA); /// Calcule la vitesse (unit) en ligne droit afin de pouvoir freiner au /// bout d'une distance (mm). double computeSpeed (double dist) const; /// Va vers un point, en formant un arc de cercle, renvois faux si atteind. /// \deprecated Utiliser goTo(). bool goToArc (double x, double y, double eps = 5.0); + protected: + /// Passe au mouvement suivant. + bool nextMovement (void); }; #endif // motor_h diff --git a/2004/i/nono/src/motor/motor_cmd.cc b/2004/i/nono/src/motor/motor_cmd.cc new file mode 100644 index 0000000..578ed3d --- /dev/null +++ b/2004/i/nono/src/motor/motor_cmd.cc @@ -0,0 +1,147 @@ +// motor_cmd.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 "motor_cmd.h" +#include "motor.h" +#include "movement_types.h" + +#include +#include + +const char motorHelp[] = + " g go (active l'asservissement)\n" + " G ungo (désactive l'asservissement)\n" + " w wait stop (attend l'arret complet)\n" + " b \n" + " mouvement basique\n" + " t va en (x, y)\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" + " h [p ]...\n" + " trace une courbe d'hermite\n"; + +bool +motorCmd (int argc, char **argv, int &i, Motor &m) +{ + switch (argv[i][0]) + { + case 'g': + std::cout << "test: go" << std::endl; + i++; + m.go (); + break; + case 'G': + std::cout << "test: ungo" << std::endl; + i++; + m.go (false); + break; + case 'w': + std::cout << "test: wait stop" << std::endl; + i++; + m.waitStopped (); + break; + case 'b': + { + i++; + double speed; + double len; + if (i >= argc) break; + speed = atof (argv[i++]); + if (i >= argc) break; + len = atof (argv[i++]); + std::cout << "test: basic " << speed << ' ' << len << std::endl; + Movement *mov = new MovementBasic (speed, len); + m.addMovement (mov); + } + break; + case 't': + { + i++; + double dX, dY; + if (i >= argc) break; + dX = atof (argv[i++]); + if (i >= argc) break; + dY = atof (argv[i++]); + std::cout << "test: goto " << dX << ' ' << dY << std::endl; + Goto *g = new GotoSimple (dX, dY); + Movement *mov = new MovementGoto (g); + m.addMovement (mov); + } + break; + case 'r': + { + i++; + double dA; + if (i >= argc) break; + dA = atof (argv[i++]); + std::cout << "test: rotation " << dA << std::endl; + Movement *mov = new MovementRotation (dA * 2 * M_PI / + 360); + m.addMovement (mov); + } + break; + case 'c': + { + i++; + double r, a; + if (i >= argc) break; + r = atof (argv[i++]); + if (i >= argc) break; + a = atof (argv[i++]); + std::cout << "test: circle " << r << ' ' << a << std::endl; + Goto *g = new GotoCircle (r, a * 2 * M_PI / 360); + Movement *mov = new MovementGoto (g); + m.addMovement (mov); + } + break; + case 'h': + { + i++; + double fa, x, y; + if (i >= argc) break; + fa = atof (argv[i++]); + std::cout << "test: hermite " << fa << ' ' << x << ' ' << y << + std::endl; + GotoHermite *g = new GotoHermite (fa * 2 * M_PI / 360); + while (argv[i][0] == 'p') + { + i++; + if (i >= argc) break; + x = atof (argv[i++]); + if (i >= argc) break; + y = atof (argv[i++]); + std::cout << "test: hermite point " << x << ' ' << y << + std::endl; + g->add (x, y); + } + Movement *mov = new MovementGoto (g); + m.addMovement (mov); + } + break; + default: + return false; + } + return true; +} + diff --git a/2004/i/nono/src/motor/motor_cmd.h b/2004/i/nono/src/motor/motor_cmd.h new file mode 100644 index 0000000..a89e94f --- /dev/null +++ b/2004/i/nono/src/motor/motor_cmd.h @@ -0,0 +1,35 @@ +#ifndef motor_cmd_h +#define motor_cmd_h +// motor_cmd.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. +// +// }}} + +class Motor; + +extern const char motorHelp[]; + +bool +motorCmd (int argc, char **argv, int &i, Motor &m); + +#endif // motor_cmd_h diff --git a/2004/i/nono/src/motor/movement_goto.cc b/2004/i/nono/src/motor/movement_goto.cc index cc720a9..efaf959 100644 --- a/2004/i/nono/src/motor/movement_goto.cc +++ b/2004/i/nono/src/motor/movement_goto.cc @@ -88,15 +88,15 @@ MovementGoto::control (void) std::cout << "movement goto: consign " << dist << ' ' << dx << ' ' << dy << std::endl; // Calcule l'erreur. - double el, ea; - t_->computeError (dx, dy, el, ea); + double el, ea, angle; + t_->computeError (dx, dy, el, ea, angle); // Pas de marche arrière. if (el < 0.0) { el = 0.0; ea = 2.0 - ea; } -std::cout << "movement goto: error " << el << ' ' << ea << std::endl; +std::cout << "movement goto: error " << el << ' ' << ea << ' ' << angle << std::endl; // Calcule les intégrales saturées. il_ += el; if (il_ > param_.is_) il_ = param_.is_; @@ -110,7 +110,7 @@ std::cout << "movement goto: error " << el << ' ' << ea << std::endl; double a = param_.kpa_ * (ea + param_.kia_ * ia_ + param_.kda_ * (ea - lea_)); std::cout << "movement goto: command " << l << ' ' << a << std::endl; - m_->speed (l, a, dist); + m_->speed (l, a, dist, angle); // Retiens l'erreur pour la dérivée. lel_ = el; lea_ = ea; diff --git a/2004/i/nono/src/motor/movement_types.h b/2004/i/nono/src/motor/movement_types.h index 901deea..ca80715 100644 --- a/2004/i/nono/src/motor/movement_types.h +++ b/2004/i/nono/src/motor/movement_types.h @@ -31,5 +31,6 @@ #include "goto_simple.h" #include "goto_circle.h" +#include "goto_hermite.h" #endif // movement_types_h diff --git a/2004/i/nono/src/motor/test_goto_hermite.cc b/2004/i/nono/src/motor/test_goto_hermite.cc index 431347a..bfa8b1e 100644 --- a/2004/i/nono/src/motor/test_goto_hermite.cc +++ b/2004/i/nono/src/motor/test_goto_hermite.cc @@ -25,10 +25,17 @@ #include "goto_hermite.h" #include +#include int main (int argc, char **argv) { + if (argc < 3) + { + std::cerr << + "test_goto_hermite - teste la généation de courbes d'hermite.\n" + " destination" << std::endl; + } GotoHermite g (0); for (int i = 1; i < argc - 1; i += 2) g.add (atof (argv[i]), atof (argv[i + 1])); diff --git a/2004/i/nono/src/motor/test_motor.cc b/2004/i/nono/src/motor/test_motor.cc index 42130bf..b393bb7 100644 --- a/2004/i/nono/src/motor/test_motor.cc +++ b/2004/i/nono/src/motor/test_motor.cc @@ -23,14 +23,12 @@ // // }}} #include "motor.h" -#include "erreur/erreur.h" #include "date/date.h" -#include "movement_types.h" +#include "motor_cmd.h" #include #include -#include -#include +#include using namespace std; @@ -39,15 +37,8 @@ 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 \n" - " mouvement basique\n" - " t va en (x, y)\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" + << motorHelp << + " attend (ms)\n" " ? cet ecran d'aide" << std::endl; } @@ -60,95 +51,31 @@ main (int argc, char **argv) int i = 1; while (i < argc) { - switch (argv[i][0]) + if (!motorCmd (argc, argv, i, m)) { - case 'g': - cout << "test: go" << endl; - i++; - m.go (); - break; - case 'G': - cout << "test: ungo" << endl; - i++; - m.go (false); - break; - case 'w': - cout << "test: wait stop" << endl; - i++; - m.waitStopped (); - break; - case 'b': - { - i++; - double speed; - double len; - if (i >= argc) break; - speed = atof (argv[i++]); - if (i >= argc) break; - len = atof (argv[i++]); - cout << "test: basic " << speed << ' ' << len << endl; - Movement *mov = new MovementBasic (speed, len); - m.setMovement (mov); - } - break; - case 't': - { - i++; - double dX, dY; - if (i >= argc) break; - dX = atof (argv[i++]); - if (i >= argc) break; - dY = atof (argv[i++]); - cout << "test: goto " << dX << ' ' << dY << endl; - Goto *g = new GotoSimple (dX, dY); - Movement *mov = new MovementGoto (g); - m.setMovement (mov); - } - break; - case 'r': - { - i++; - double dA; - if (i >= argc) break; - dA = atof (argv[i++]); - 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; - default: - int s; - s = atoi (argv[i++]); - if (s == 0) + switch (argv[i][0]) { + case '?': syntax (); - return 1; - } - cout << "test: sleep " << s << endl; - for (int j = 0; j < s * 1000 / 50; ++j) - { - m.ok (); - Date::wait (50); + return 0; + default: + int s; + s = atoi (argv[i++]); + if (s == 0) + { + syntax (); + return 1; + } + cout << "test: sleep " << s << endl; + int start; + start = Date::getInstance ().start (); + while (Date::getInstance ().start () < start + s) + { + m.ok (); + Date::wait (50); + } + break; } - break; } m.waitEnd (); } diff --git a/2004/i/nono/src/motor/tracker.cc b/2004/i/nono/src/motor/tracker.cc index 381ed32..4bd0e7b 100644 --- a/2004/i/nono/src/motor/tracker.cc +++ b/2004/i/nono/src/motor/tracker.cc @@ -75,6 +75,13 @@ Tracker::getAngleDiff (double a) const return angleNorm (angle_ - a); } +/// Récupère la distance à parcourir avec chaque roue pour parcourir un angle. +double +Tracker::getAngleDistance (double angleDiff) const +{ + return 0.5 * f_ * fabs (angleDiff); +} + /// Récupère les coordonnées d'un point à moins de dist (mm), dans la /// direction de (dx, dy). void @@ -101,10 +108,12 @@ Tracker::getPoint (double dx, double dy, double &x, double &y, double dist) /// Calcule l'erreur linéaire et angulaire pour atteindre un point. /// (x, y) : point à atteindre (mm). /// el, ea : erreur linéaire et angulaire (-1..+1). +/// a : angle. void -Tracker::computeError (double x, double y, double &el, double &ea) const +Tracker::computeError (double x, double y, double &el, double &ea, double &a) + const { - // Calcule la diférence avec la cible voulue. + // Calcule la différence avec la cible voulue. double eX = x - posX_; double eY = y - posY_; // Calcule la distance. @@ -120,6 +129,8 @@ Tracker::computeError (double x, double y, double &el, double &ea) const // Calcule l'erreur angulaire (projection sur la direction // perpendiculaire). ea = eX * -sa + eY * ca; + // Calcule l'angle. + a = ea > 0.0 ? acos (el) : -acos (el); } /// Calcule l'erreur angulaire pour atteindre un angle, retourne faux si diff --git a/2004/i/nono/src/motor/tracker.h b/2004/i/nono/src/motor/tracker.h index b58bc41..96cf6f0 100644 --- a/2004/i/nono/src/motor/tracker.h +++ b/2004/i/nono/src/motor/tracker.h @@ -57,6 +57,9 @@ class Tracker double getAngle (void) const { return angle_; } /// Get distance. double getDistance (double x, double y) const; + /// Récupère la distance à parcourir avec chaque roue pour parcourir un + /// angle. + double getAngleDistance (double angleDiff) 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) @@ -68,7 +71,9 @@ class Tracker /// Calcule l'erreur linéaire et angulaire pour atteindre un point. /// (x, y) : point à atteindre (mm). /// el, ea : erreur linéaire et angulaire (-1..+1). - void computeError (double x, double y, double &el, double &ea) const; + /// a : angle. + void computeError (double x, double y, double &el, double &ea, double &a) + const; /// Calcule l'erreur angulaire pour atteindre un angle, retourne faux si /// atteind. /// a : angle à atteindre (rad). -- cgit v1.2.3