// qdai.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 "qdai.h" #include "motor/movement_types.h" #include "motor/movement_reposition.h" #include "date/date.h" #include #include /// Constructeur. QdAi::QdAi (void) : ana_ (gspp_), ga_ (motor_.getAsserv ()), gc_ (ga_, gspp_, ga_.getNbIo ()), gp_ (gc_), v4l_ ("/dev/video", ImageLoader::yuv), lastCapture_ (0), front_ (135), bigFront_ (182), width_ (260), rear_ (135) { } /// Initialise le robot. void QdAi::init (void) { /// Calibre la camera. //v4l_.calibrate (); /// Asservi les moteurs. motor_.go (); } /// Arrète le robot. void QdAi::stop (void) { /// Attend l'arret complet de l'appareil. motor_.waitStopped (); } /// Lance le robot. int QdAi::run (void) { int state = 0; int oldstate = -1; bool stop = false; int gpioJack = gp_.getByName ("jack"); int gpioRoul = gp_.getByName ("roul"); int gpioRoulS = gp_.getByName ("rouls"); int gpioContactL = gp_.getByName ("contactl"); int gpioContactR = gp_.getByName ("contactr"); int anaSharpL = ana_.getByName ("ana1"); int anaSharpR = ana_.getByName ("ana0"); while (!stop && Date::getInstance ().round () < roundDuration_) { // Mises à jour. update (); if (oldstate != state) { std::cout << "qdai: " << state << std::endl; oldstate = state; } // Automate. switch (state) { case 0: // Initialise le matos. gspp_.moveTo (1, 5); std::cout << '\b' << std::endl; if (gp_.get (gpioJack)) state = 1; break; case 1: // Début de match. Date::getInstance ().startRound (); gp_.set (gpioRoul, 1); state = 1000; break; // Parcours balle gratuite. case 1000: // Direction première balle. { Goto *g = new GotoSimple (900, 1500); Movement *m = new MovementGoto (g); motor_.addMovement (m); if (!updateToEnd ()) break; } state = 1001; break; case 1001: // Direction en-but. { Goto *g = new GotoSimple (1200, 1800); Movement *m = new MovementGoto (g); motor_.addMovement (m); if (!updateToEnd ()) break; } { Goto *g = new GotoSimple (2400, 1800); Movement *m = new MovementGoto (g); motor_.addMovement (m); if (!updateToEnd ()) break; } state = 1002; break; case 1002: // Levage benne. gspp_.moveTo (1, 57); // Raprochement en-but. { Movement *m = new MovementReposition (0.4, gp_, gpioContactL, gpioContactR, ana_, anaSharpL, anaSharpR); motor_.addMovement (m); if (!updateToEnd ()) break; } // Recalage du tracker. motor_.getTracker ().adjustX (9 * 300 - front_); motor_.getTracker ().adjustAngle (0); state = 1003; break; case 1003: // Décharge. gp_.reset (gpioRoul); if (!updateWait (200)) break; gp_.set (gpioRoulS, 1); if (!updateWait (200)) break; gp_.set (gpioRoul, 1); if (!updateWait (3000)) break; state = 1004; break; case 1005: // Recule et retourne. { Movement *m = new MovementBasic (-1.0, 100); motor_.addMovement (m); if (!updateToEnd ()) break; } { Movement *m = new MovementRotation (-2/3 * M_PI); motor_.addMovement (m); if (!updateToEnd ()) break; } stop = true; break; // Parcours découverte. case 2010: // Recherche de balle. { Movement *m = new MovementRoundCruise (0.1); motor_.addMovement (m); } // Attendre de trouver une balle. state = 2020; break; case 2020: // Trackage de balle. { } state = 2021; break; case 2021: // Gobage de la balle. { Movement *m = new MovementBasic (0.7, 300); motor_.addMovement (m); updateToEnd (); } state = 2030; break; case 2030: // Goto zone d'essai. { // Goto près de la zone d'essais. { Goto *g = new GotoSimple (8 * 300, 450); Movement *m = new MovementGoto (g); motor_.addMovement (m); updateToEnd (); } // Rotation vers la zone d'essais. { Movement *m = new MovementRotation (0.0); motor_.addMovement (m); updateToEnd (); } // Goto la zone d'essais. { Goto *g = new GotoSimple (8 * 300 + 15, 450); Movement *m = new MovementGoto (g); motor_.addMovement (m); updateToEnd (); } } state = 2040; break; case 2040: // Essai. { stop = true; // Lever la benne. // Sortir les balles. // Baisser la benne. } break; default: std::cout << "qdai: unknown state" << std::endl; } } return 0; } /// Mises à jour périodiques. void QdAi::update (void) { // Mise à jour moteur. motor_.waitOk (); // Mise à jour capteurs. gspp_.update (); // Mise à jour camera. if (lastCapture_ + captureInterval_ < Date::getInstance ().start ()) { } } /// Mises à jour jusqu'à fin du mouvement en cours. Renvois faux à la fin /// du match. bool QdAi::updateToEnd (void) { while (!motor_.end ()) { if (Date::getInstance ().round () >= roundDuration_) return false; update (); Date::wait (50); } return true; } /// Mises à jour jusqu'à timeout. Renvois faux à la fin du match. bool QdAi::updateWait (int t) { int start; start = Date::getInstance ().start (); while (Date::getInstance ().start () < start + t) { if (Date::getInstance ().round () >= roundDuration_) return false; update (); Date::wait (50); } return true; } int main (int argc, char **argv) { QdAi qdai_; qdai_.init (); qdai_.run (); qdai_.stop (); }