// 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" #include "config/config.h" MovementGotoParam MovementGoto::param_; /// Constructeur, charge les paramètres depuis la Config. MovementGotoParam::MovementGotoParam (void) : eps_ (10.0), dist_ (100.0), kpl_ (1.0), kpa_ (1.0), kil_ (0.0), kia_ (0.0), is_ (20.0), kdl_ (0.0), kda_ (0.0) { // Lit la conf. Config rc ("rc/movement/goto"); while (!rc.eof ()) { if (!( rc.get ("epsilon", eps_) || rc.get ("distance", dist_) || rc.get ("kpl", kpl_) || rc.get ("kpa", kpa_) || rc.get ("kil", kil_) || rc.get ("kia", kia_) || rc.get ("is", is_) || rc.get ("kdl", kdl_) || rc.get ("kda", kda_) )) rc.noId (); } } /// Constructeur. /// go : objet Goto, detruit dans le destructeur. MovementGoto::MovementGoto (Goto *go) : goto_ (go), il_ (0.0), ia_ (0.0), lel_ (0.0), lea_ (0.0) { } /// Destructeur. MovementGoto::~MovementGoto (void) { delete goto_; } /// Initialise le mouvement, appelé juste quand le mouvement est mis en /// service. void MovementGoto::init (const Tracker &t, Asserv &a, Motor &m) { Movement::init (t, a, m); goto_->init (t); } /// Controlle la vitesse, retourne faux si mouvement terminé. bool MovementGoto::control (void) { // Récupère la distance et le prochain point. double dist, dx, dy; if (!goto_->get (*t_, param_.dist_, param_.eps_, dist, dx, dy)) return false; //std::cout << "movement goto: consign " << dist << ' ' << dx << ' ' << dy << std::endl; // Calcule l'erreur. double el, ea, angle; t_->computeError (dx, dy, el, ea, angle); // Pas de marche arrière. if (el < 0.0) { el = 0.0; ea = 2.0 - ea; } //std::cout << "movement goto: error " << el << ' ' << ea << ' ' << angle << std::endl; // Calcule les intégrales saturées. il_ += el; if (il_ > param_.is_) il_ = param_.is_; else if (il_ < -param_.is_) il_ = -param_.is_; ia_ += ea; if (ia_ > param_.is_) ia_ = param_.is_; else if (ia_ < -param_.is_) ia_ = -param_.is_; // Commande les moteurs. double l = param_.kpl_ * (el + param_.kil_ * il_ + param_.kdl_ * (el - lel_)); double a = param_.kpa_ * (ea + param_.kia_ * ia_ + param_.kda_ * (ea - lea_)); //std::cout << "movement goto: command " << l << ' ' << a << std::endl; m_->speed (l, a, dist, angle); // Retiens l'erreur pour la dérivée. lel_ = el; lea_ = ea; return true; }