// movement_goto.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 "movement_goto.h" #include "tracker.h" #include "motor.h" // Constructeur. MovementGoTo::MovementGoTo (double x, double y, double eps, double dist, double kpl, double kpa, double kil, double kia, double kdl, double kda) : dX_ (x), dY_ (y), eps_ (eps), dist_ (dist), kpl_ (kpl), kpa_ (kpa), kil_ (kil), kia_ (kia), kdl_ (kdl), kda_ (kda), il_ (0.0), ia_ (0.0), lel_ (0.0), lea_ (0.0) { } // Controlle la vitesse, retourne faux si mouvement terminé. bool MovementGoTo::control (void) { // Calcule l'erreur. double el, ea; if (!t_->computeError (dX_, dY_, el, ea, eps_, dist_)) { return false; } // Pas de marche arrière. if (el < 0.0) el = 0.0; std::cout << "movement goto error " << el << ' ' << ea << std::endl; // Calcule les intégrales saturées. il_ += el; if (il_ > 1000) il_ = 1000; else if (il_ < -1000) il_ = -1000; ia_ += ea; if (ia_ > 1000) ia_ = 1000; else if (ia_ < -1000) ia_ = -1000; // Commande les moteurs. double l = kpl_ * (el + kil_ * il_ + kdl_ * (el - lel_)); double a = kpa_ * (ea + kia_ * ia_ + kda_ * (ea - lea_)); m_->speed (l - a, l + a); // Retiens l'erreur pour la dérivée. lel_ = el; lea_ = ea; return true; }