// 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), 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 ("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) { instance_ = 0; delete movement_; } /// 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 (!movement_->control ()) { delete movement_; movement_ = 0; asserv_.stop (); } } /// Choisi le mouvement en cours. void Motor::setMovement (Movement *m) { delete movement_; movement_ = m; m->init (tracker_, asserv_, *this); } /// Teste si les mouvements sont terminés. bool Motor::end (void) { ok (); return !movement_; } /// 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). void Motor::speed (double l, double a, double dist) { speedLimit (l - a, l + a, computeSpeed (dist)); } /// Paramètre la vitesse des moteurs (-1..+1). void Motor::speed (double l, double r) { speedLimit (l, r, maxSpeed_); } /// Paramètre la vitesse des moteurs (-1..+1) avec une vitesse limite /// (unit). void Motor::speedLimit (double l, double r, double vLimit) { // Il faut un minimum tout de même. La politique, c'est on avance. if (vLimit < minSpeed_) vLimit = 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); // 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); } // Teste le dépassement de vitesse maximal. if (vL > vLimit) { vL = static_cast (vLimit); vR = (int) (r / l * vL); } else if (vL < -vLimit) { vL = static_cast (-vLimit); vR = (int) (r / l * vL); } if (vR > vLimit) { vR = static_cast (vLimit); vL = (int) (l / r * vR); } else if (vR < -vLimit) { vR = static_cast (-vLimit); vL = (int) (l / r * vR); } // Envois la commande. log_ (Log::debug) << "speed " << l << ' ' << r << ' ' << vL << ' ' << vR << ' ' << vLimit << 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; } }