// 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), as_ (1.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 ("speedscale", vs_) || rc.get ("accelscale", as_) || rc.get ("maxspeed", maxSpeed_) || rc.get ("minspeed", minSpeed_) )) rc.noId (); } asserv_.reset (); } /// Destructeur. Motor::~Motor (void) { instance_ = 0; delete movement_; } /// Appelée lors d'une mise à jour des compteurs. void Motor::updateCounter (int l, int r) { double dL = l * s_; double dR = r * s_; tracker_.update (dL, dR); 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 des moteurs (-1..+1). void Motor::speed (double l, double r) { 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) ((vs_ > 0 ? l : -l) * maxSpeed_); vR = (int) ((vs_ > 0 ? r : -r) * maxSpeed_); } // Teste le dépassement de vitesse maximal. if (vL > maxSpeed_) { vL = maxSpeed_; vR = (int) (r / l * vL); } else if (vL < -maxSpeed_) { vL = -maxSpeed_; vR = (int) (r / l * vL); } if (vR > maxSpeed_) { vR = maxSpeed_; vL = (int) (l / r * vR); } else if (vR < -maxSpeed_) { vR = -maxSpeed_; vL = (int) (l / r * vR); } // Envois la commande. log_ (Log::debug) << "speed " << l << ' ' << r << ' ' << vL << ' ' << vR << std::endl;; asserv_.speed (vL, vR); } /// Va vers un point, renvois faux si atteind. bool Motor::goTo (double x, double y, double eps/*5.0*/) { return false; } /// 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 * asserv_.getAccel () * as_ * l); else l = -sqrt (2.0 * asserv_.getAccel () * as_ * -l); if (r > 0.0) r = sqrt (2.0 * asserv_.getAccel () * as_ * r); else r = -sqrt (2.0 * asserv_.getAccel () * as_ * -r); log_ << ' ' << l << ' ' << r << std::endl; speed (l, r); return true; } else { asserv_.stop (); return false; } }