// 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 #include // Pointeur vers l'instance unique. Motor *Motor::instance_ = 0; // Constructeur. Motor::Motor (void) : asserv_ (*this), 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.isId ("scale")) { rc.getId (); rc >> s_; } else if (rc.isId ("speedscale")) { rc.getId (); rc >> vs_; } else if (rc.isId ("accelscale")) { rc.getId (); rc >> as_; } else if (rc.isId ("maxspeed")) { rc.getId (); rc >> maxSpeed_; } else if (rc.isId ("minspeed")) { rc.getId (); rc >> minSpeed_; } else 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. void Motor::speed (double l, double r) { int vL = (int) (l / vs_); int vR = (int) (r / vs_); // 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) (l / vs_); vR = (int) (r / vs_); } // 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. std::cout << "motor 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. bool Motor::goToArc (double x, double y, double eps/*5.0*/) { double l, r; if (tracker_.computeArcs (x, y, l, r, eps)) { std::cout << "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); std::cout << ' ' << l << ' ' << r << std::endl;; speed (l, r); return true; } else { asserv_.stop (); return false; } }