// movement_reposition.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_reposition.h" #include "motor.h" #include "tracker.h" #include "date/date.h" /// Constructeur. /// speed : vitesse (-1..+1). /// gpio : gpio d'entré. /// gpil : gpi gauche. /// gpir : gpi droite. /// ana : analog d'entrée. /// anal : ana gauche. /// anar : ana droite. MovementReposition::MovementReposition (double speed, Gpio &gpio, int gpil, int gpir, Analog &ana, int anal, int anar) : speed_ (speed), gpio_ (gpio), gpil_ (gpil), gpir_ (gpir), ana_ (ana), anal_ (anal), anar_ (anar), state_ (0) { } /// Initialise le mouvement, appelé juste quand le mouvement est mis en /// service. void MovementReposition::init (const Tracker &t, Asserv &a, Motor &m) { Movement::init (t, a, m); stepDate_ = Date::getInstance ().start (); } /// Controlle la vitesse, retourne faux si mouvement terminé. bool MovementReposition::control (void) { std::cout << "reposition: " << state_ << std::endl; switch (state_) { case 0: { if (stepDate_ + 5000 < Date::getInstance ().start ()) return false; double dl = ana_.get (anal_); double dr = ana_.get (anar_); double dm = dl < dr ? dl : dr; std::cout << "reposition: 0 " << dl << ' ' << dr << std::endl; m_->speedLR (dl > 62.0 ? speed_ : -speed_ * 0.5, dr > 62.0 ? speed_ : -speed_ * 0.5, dm - 62.0, M_PI); if (dr <= 62.0 && dl <= 62.0) { state_ = 1; stepDate_ = Date::getInstance ().start (); step1_ = Point (t_->getX (), t_->getY ()); } } return true; case 1: { if (stepDate_ + 5000 < Date::getInstance ().start ()) return false; double d = 100.0 - step1_.distTo (Point (t_->getX (), t_->getY ())); bool l = gpio_.get (gpil_); bool r = gpio_.get (gpir_); std::cout << "reposition: 1 " << l << ' ' << r << std::endl; if (l && r) { state_ = 2; return false; } m_->speedLR (!l ? speed_ : -speed_ * 0.05, !r ? speed_ : -speed_ * 0.05, d - 50.0, M_PI); } return true; case 2: { // XXX: Inutilisé. bool l = gpio_.get (gpil_); bool r = gpio_.get (gpir_); std::cout << "reposition: 2 " << l << ' ' << r << std::endl; if (!l && !r) return false; m_->speed (!l ? speed_ * 0.05 : -speed_ * 0.05, !r ? speed_ * 0.05 : -speed_ * 0.05); } return true; } return false; }