// motor.cc // nono - programme du robot 2004. {{{ // // Copyright (C) 2003 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.h" #include "config/config.h" #include // Pointeur vers l'instance unique. Motor *Motor::instance_ = 0; /// Constructeur. Motor::Motor (void) : asserv_ (*this), log_ ("motor"), s_ (1.0), vs_ (1.0), a_ (1.0), fact_ (0.5), brakeDistance_ (0.0), movement_ (0), maxSpeed_ (0x40), minSpeed_ (0x04) { // Mémorise le pointeur d'instance. instance_ = this; // Lit la conf. Config rc ("rc/motor"); while (!rc.eof ()) { if (!( rc.get ("scale", s_) || rc.get ("fact", fact_) || rc.get ("maxspeed", maxSpeed_) || rc.get ("minspeed", minSpeed_) )) rc.noId (); } // Initialise l'échelle. vs_ = s_ * asserv_.getSpeedFactor (); a_ = (s_ > 0 ? s_ : -s_) * asserv_.getAccelFactor (); brakeDistance_ = 0.5 * (maxSpeed_ * maxSpeed_ * vs_ * vs_) / a_; log_ (Log::debug) << "scale " << vs_ << ' ' << a_ << ' ' << brakeDistance_ << std::endl; // C'est partit ! asserv_.reset (); } /// Destructeur. Motor::~Motor (void) { clearMovement (); instance_ = 0; } /// Appelée lors d'une mise à jour des compteurs. /// l, r : mise à jour moteur gauche et droit (unit). /// zero : vrai si le mouvement est à considérer comme nul. void 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 ()) { asserv_.speed (0, 0); return; } } if (movement_) { while (!movement_->control ()) { if (!nextMovement ()) { // C'est la fin. return; } } } } /// Ajoute un mouvement à la file d'attente. void Motor::addMovement (Movement *m) { movementQueue_.push_back (m); } /// Vide la file d'attente. void Motor::clearMovement (void) { delete movement_; MovementQueue::iterator first = movementQueue_.begin (); MovementQueue::iterator last = movementQueue_.end (); for ( ; first != last; ++first) delete *first; } /// Teste si les mouvements sont terminés. bool Motor::end (void) { ok (); return !movement_ && movementQueue_.empty (); } /// Attend la fin de tout les mouvements. void Motor::waitEnd (void) { do { waitOk (); } while (!end ()); } /// Teste si les moteurs sont arrétés. bool Motor::stopped (void) { ok (); return tracker_.stopped (); } /// Attend que les moteurs soient arrétés. void Motor::waitStopped (void) { do { waitOk (); } 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) et au bout de /// l'angle (rad). void Motor::speed (double l, double a, double dist, double angle) { double liml, lima; liml = computeSpeed (dist); lima = computeSpeed (tracker_.getAngleDistance (angle)); speedLimit (l - a, l + a, liml, lima); } /// Paramètre la vitesse linéaire (-1..+1) gauche et droite. Limite la /// vitesse pour pouvoir freiner au bout de dist (mm) et au bout de /// l'angle (rad). void Motor::speedLR (double l, double r, double dist, double angle) { double liml, lima; liml = computeSpeed (dist); lima = computeSpeed (tracker_.getAngleDistance (angle)); speedLimit (l, r, liml, lima); } /// Paramètre la vitesse angulaire (-1..+1). Limite la vitesse pour /// pouvoir freiner au bout de l'angle (rad). void Motor::rotate (double a, double angle) { double lima; lima = computeSpeed (tracker_.getAngleDistance (angle)); speedLimit (-a, +a, maxSpeed_, lima); } /// Paramètre la vitesse des moteurs (-1..+1). void Motor::speed (double l, double r) { speedLimit (l, r, maxSpeed_, 2 * maxSpeed_); } /// Paramètre la vitesse des moteurs (-1..+1) avec une vitesse limite /// linéaire en angulaire (unit). void 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 (vLimitL < minSpeed_) vLimitL = minSpeed_; if (vLimitA < minSpeed_) vLimitA = minSpeed_; // Convertie le (-1..+1) en (unit). 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_ * fact_); vR = (int) ((vs_ > 0 ? r : -r) * maxSpeed_ * fact_); } // Teste le dépassement de vitesse maximale linéaire. if (vL > vLimitL) { vL = static_cast (vLimitL); vR = (int) (r / l * vL); } else if (vL < -vLimitL) { vL = static_cast (-vLimitL); vR = (int) (r / l * vL); } if (vR > vLimitL) { vR = static_cast (vLimitL); vL = (int) (l / r * vR); } else if (vR < -vLimitL) { 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 << ' ' << vLimitL << ' ' << vLimitA << std::endl;; asserv_.speed (vL, vR); } /// Calcule la vitesse (unit) en ligne droit afin de pouvoir freiner au bout /// d'une distance (mm). double Motor::computeSpeed (double dist) const { if (dist <= 0.0) return 0.0; if (dist > brakeDistance_) return maxSpeed_; else return sqrt (2.0 * dist * a_) / (vs_ > 0 ? vs_ : -vs_); } /// Va vers un point, en formant un arc de cercle, renvois faux si atteind. /// \deprecated Utiliser goTo(). bool Motor::goToArc (double x, double y, double eps/*5.0*/) { double l, r; if (tracker_.computeArcs (x, y, l, r, eps)) { log_ (Log::debug) << "goto arc " << l << ' ' << r; if (l > 0.0) l = sqrt (2.0 * a_ * l); else l = -sqrt (2.0 * a_ * -l); if (r > 0.0) r = sqrt (2.0 * a_ * r); else r = -sqrt (2.0 * a_ * -r); log_ << ' ' << l << ' ' << r << std::endl; speed (l, r); return true; } else { asserv_.stop (); return false; } } /// 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; }