// ai.cc // robert - programme du robot 2005. {{{ // // Copyright (C) 2005 Nicolas Haller // // Robot APB Team/Efrei 2005. // 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 #include "ai.hh" #include "config/config.hh" #include "timer/timer.hh" #include "ovision/see/ovision.hh" static void callback (void) { } /// Constructeur Ai::Ai(const Config & config) :motor_(config), es_(config), schedulableMotor_ (callback, motor_.getFd ()), schedulableEs_ (callback, es_.getFd ()), roundDuration_(config.get("ai.roundDuration")), vitesseAsc_(config.get("ai.vitesseAsc")), tempoDebutMatch_ (config.get ("ai.tempoDebutMatch")) { scheduler_.insert (schedulableMotor_); scheduler_.insert (schedulableEs_); } /// Destructeur Ai::~Ai (void) { // On réinitialise // Initialise les moteurs motor_.init(); // initialise la carte es es_.init(); // On sync do { scheduler_.schedule(500, true); } while (!sync ()); } // *** Fonctions de communication avec motor et es ***/*{{{*/ /// Initialise le robot void Ai::init(void) { // Initialise les moteurs std::cout << "init des moteurs\n"; motor_.init(); // initialise la carte es std::cout << "init de l'es\n"; es_.init(); // on init la vision std::cout << "init d'ovision\n"; oVision_.init(motor_.colorState() ? Group::redSkittle : Group::greenSkittle); while (!update()); } /// stop le robot void Ai::stop(void) { // Stop les moteurs motor_.stop(); // On sync do { update (); } while (!motor_.idle ()); } /// Attend des données. void Ai::wait (int t) { motor_.wait(t); } /// Fonction de temporisation void Ai::temporisation(int t) { int time = Timer::getProgramTime(); time += t; do { update(); } while (time > Timer::getProgramTime()); } // Attend le jack entré (false) ou sorti (true). void Ai::waitJack (bool out) { do { update(); } while (motor_.jackState() != out); } /// Rejoint un point. (argument en mm) void Ai::goTo (double x, double y ,double a) { motor_.goTo(x,y,a); do { update(); } while(!motor_.idle()); } /// Recale contre une bordure. void Ai::recale (void) { motor_.recalage(); do { update(); } while(!motor_.idle()); } /// Mouvement basic. void Ai::basic (double d) { motor_.linearMove(d); do { update(); } while (!motor_.idle()); } /// Rotation (argument en radian) void Ai::rotation (double a) { // On vérifie que ca ne fasse pas moins de trois degré // On récupère l'angle double destNorm; // On normalise si besoin if (a < 0) destNorm += 2 * M_PI; // On récupère la différence d'angle double diff = motor_.getA () - destNorm; double limit = 3 / 360 * 2 * M_PI; // 3 degré en radian if (diff > limit || diff < - limit) { motor_.rotation(a); do { update(); } while (!motor_.idle()); } } /// Monte(vrai) ou descend(faux) l'ascenceur void Ai::ascenceur (bool monte) { if (monte) es_.monterAsc(); else es_.descendreAsc(); do { update(); } while (!es_.ascIsIdle()); } /// Désactive les ventouse void Ai::ventouses (void) { es_.ventouses(); while (!update()); } /// Attend une mise à jour bool Ai::update (void) { scheduler_.schedule(500, true); bool retour = sync (); // On vérifie que le match n'est pas fini if (Timer::getRoundTime () > roundDuration_) throw std::runtime_error ("Fin de match, merci d'avoir participé !"); return retour; } /// Synchronise les données avec motor et es. bool Ai::sync (void) { bool motor = motor_.sync(); bool es = es_.sync(); if (es && motor) return true; else return false; } /// Reset la PWM à zéro. void Ai::resetPwm (void) { motor_.setPwm (0, 0); // synchronise while (!update()); } void Ai::setMaxSpeed (int l, int r) { motor_.setMaxSpeed (l, r); while (!update ()); } void Ai::setAccel (int a) { motor_.setAccel (a); while (!update ()); } /// Renvoye l'état du bouton de sélection de couleur. bool Ai::colorBouton (void) { return motor_.colorState (); } /// Renvoye si l'objet devant nous est trop prêt. bool Ai::objectInFront (void) { return es_.obsTropPres (); } /*}}}*/ // *** Les runs ***/*{{{*/ // Balistique non fiable. void Ai::runBalNonFiable (void) { // Lancement du placement pour les matchs initMatch (); // Protection des quilles devant le pont protectQuillePont (); } // Balistique fiable. void Ai::runBalFiable (void) { // Lancement du placement pour les matchs initMatch (); // Goto pour avance puis tourner de - PI/2 // Algo de recherche de quille sur ligne du fond } // Match de parcourt du fond du terrain void Ai::runProtectFond (void) { // Lancement du placement pour les matchs initMatch (); // Protection des quilles devant le pont parcourtFond (); } void Ai::runVisionLine(void) { init(); initTest(); do { scanVision(true); basic(100); }while(motor_.getY() < 1700); } // Pour les Tests. void Ai::runTest (void) { initTest (); // On change la vitesse max pour reculer std::cout << "On change la vitesse à 2" << std::endl; setMaxSpeed (20, -1); setAccel (30); // Taper la boule std::cout << "On recule à fond de 460mm" << std::endl; basic(-300); std::cout << "Arret d'urgence" << std::endl; //setAccel (0); //stop (); // On restaure la vitesse temporisation (400); init (); std::cout << "On restaure la vitesse à 8" << std::endl; setAccel (96); setMaxSpeed (8, -1); std::cout << "On avance pour voir" << std::endl; basic(+300); } /*}}}*/ // *** Les inits (test/match) ***/*{{{*/ // Séquences d'initiailisation pour les tests. void Ai::initTest (void) { // On init le robot init(); // Reset la PWM à zéro si on a fait de la merde en le posant sur la table resetPwm (); } /// Initialise le robot pour faire un match (placement). void Ai::initMatch (void) { //Initialise le robot std::cout << "On init le robot" << std::endl; init(); // Attend l'entrée du jack "pret à initialiser" std::cout << "On attend le jack" << std::endl; waitJack(false); std::cout << "Le jack est mis, on réinit la PWM" << std::endl; // Reset la PWM à zéro si on a fait de la merde en le posant sur la table resetPwm (); // Attend la première sortie du jack(init) waitJack(true); std::cout << "Le jack est sorti" << std::endl; //On fait une rotation pour recaler y std::cout << "On tourne -> -PI/2" << std::endl; rotation(-M_PI/2); // On recule dans le mur std::cout << "On se recale" << std::endl; recale (); motor_.setPosition(motor_.getX(), -60, -M_PI / 2); // On avance vers la position de départ en y std::cout << "On avance de 155mm" << std::endl; basic (155); // 215 - 60 = 155 // On rotate de 90 ° std::cout << "On rotate -> 0" << std::endl; rotation (0); // On se recale vers x std::cout << "On se recale" << std::endl; recale(); motor_.setPosition(60, motor_.getY(), 0); std::cout << "On attend le jack" << std::endl; // Attend le jack "pret à partir" waitJack(false); std::cout << "Le jack est mis" << std::endl; // Attend la seconde sortie du jack(Daniel) waitJack(true); std::cout << "Le jack est sortie" << std::endl; // On lance le timer de début de match std::cout << "Lancement du match" << std::endl; Timer::startRound(); // On temporise pour laisser Taz sortir std::cout << "Attente de " << tempoDebutMatch_ << " secondes" << std::endl; temporisation(tempoDebutMatch_); }/*}}}*/ // Parcourt la ligne du fond, en aveugle pour relever des quilles et les poser derrière soit void Ai::parcourtFond (void) { /*** Préplacement ***/ std::cout << "Placement dans la ligne - goTo 200" << std::endl; goTo (200, motor_.getY (), - M_PI / 2); //1 degré -> 0.017 std::cout << "On avance de 80 cm" << std::endl; basic (800); /*** Fin du préplacement ***/ /*** Boucle de parcours de la ligne de fond ***/ // On s'arrête dans le dernire carreaux while (motor_.getY () > -1700) { std::cout << "On recule un peu" << std::endl; basic (-10); std::cout << "On lève" << std::endl; ascenceur (true); std::cout << "Vitesse lente de rotation 1" << std::endl; setMaxSpeed (-1, 1); // XXX Ça pue un peu quand même ça, on ne peut pas faire en deux fois std::cout << "On tourne un peu" << std::endl; rotation (M_PI); std::cout << "On fini le tour" << std::endl; rotation (M_PI / 2); std::cout << "On attend 3 secondes" << std::endl; temporisation (3000); std::cout << "On lache" << std::endl; ventouses (); std::cout << "On attend 1.5 secondes" << std::endl; temporisation (1500); std::cout << "On restaure les vitesses" << std::endl; setMaxSpeed (-1, 4); std::cout << "On recule de 10 cm" << std::endl; basic (-100); std::cout << "On descend" << std::endl; ascenceur (false); std::cout << "On se retourne" << std::endl; rotation ( -M_PI / 2); std::cout << "On avance de 20 cm" << std::endl; basic (200); } std::cout << "Fini : " << Timer::getRoundTime () << std::endl; } // *** Fonctions de vision *** // On scan tout autour du robot void Ai::scanVision (bool followLine) /// XXX mettre un argument { std::cout << "On commence le scannage de quiles" << std::endl; double angleDepart = motor_.getA(); std::vector > skittles; for (int i = 0; i < (followLine ? 1 : 4) ; i++) { std::cout << "On prend la photo" << std::endl; // On prend la photo oVision_.takeShoot(); std::cout << "On update ovision" << std::endl; // On analyse la photo oVision_.update(); std::cout << "On sauvegarde la lsite de quilles" << std::endl; // On mets la liste brute dans le vector skittles.push_back(oVision_.getSkittles()); // on tournicote if (!followLine) motor_.rotation(angleDepart + ((i + 1) * M_PI/4)); } analyseSkittles(skittles); remonteWithCam (); } // XXX Aucun moyen de tourner pour l'instant (donc followLine only) // On analyse les quilles pour ne garder que les prétendants void Ai::analyseSkittles(std::vector > skittlesBrut) { std::cout << "AnalyseSkittles" << std::endl; goodSkittles_.clear(); // On regarde pour tout les angles for(std::vector >::iterator it = skittlesBrut.begin(); it != skittlesBrut.end(); it++) { // On regarde zone par zone for(std::vector::iterator it2 = it->begin(); it2 != it->end(); it2++) { if (!it2->alone) continue; if (it2->vertical) continue; if (it2->partial) // XXX to manage continue; if (it2->small) continue; std::cout << "On a une quille une gagnante !" << std::endl; goodSkittles_.push_back(*it2); } } } // On remonte la ze quille la meilleur détecté par la cam void Ai::remonteWithCam(void) { int x, y , angle, dist; std::cout << "remonteWithCam" << std::endl; if (goodSkittles_.size () > 0) { // On récupère la quille std::cout << "On récupère la quille" << std::endl; Zone zeSkittles = goodSkittles_.back(); // On récupère sa position oVision_.getLoc(zeSkittles, x, y, angle, dist); std::cout << "x : " << x << ", y : " << y << ", a : " << angle << ", d : " << dist << "." << std::endl; // On enlève la partie avant du robot du au calibrage dist -= 90; // On pivote de l'angle demandée // XXX position relative par rapport à la prise de photo std::cout << "Rotation" << std::endl; rotation(motor_.getA() + angle); std::cout << "Max speed" << std::endl; setMaxSpeed(2,1); std::cout << "basic dist/d" << std::endl; basic(dist); std::cout << "Ascenceur +" << std::endl; ascenceur(true); // basic (-dist - 5); // rotation(M_PI); // rotation(M_PI / 2); // basic(30); // XXX vérifier valeur ventouses(); std::cout << "Max speed" << std::endl; setMaxSpeed(8, 4); std::cout << "basic - 30" << std::endl; basic(-30); // XXX Vérifier valeur std::cout << "Ascenceur -" << std::endl; ascenceur(false); // rotation(3 * M_PI / 4); } } // Va du point de départ au pont pour protéger les quilles. void Ai::protectQuillePont (void) { // On avance pour pouvoir aller vers la boule parallèlement au socle std::cout << "On avance de 540mm" << std::endl; basic(540); // 600 - 60 // On se place derrière les quilles en arrière std::cout << "GoTo 1225, -1200 -> Pi" << std::endl; goTo(1225, -1200, M_PI); // On change la vitesse max pour reculer std::cout << "On change la vitesse linéaire à 2" << std::endl; setMaxSpeed (2, -1); // Taper la boule std::cout << "On recule lentement de 14 cm" << std::endl; basic(-140); // On restaure la vitesse std::cout << "On restaure la vitesse à 8" << std::endl; setMaxSpeed (8, -1); // On attend un peu pour protéger std::cout << "On temporise de 2.5 secondes" << std::endl; temporisation (2500); // On se barre des quilles std::cout << "On avance de 20 cm" << std::endl; basic (200); // On retourne à l'origine std::cout << "GoTo 600, -300 -> Pi" << std::endl; goTo (600, -300, M_PI); // On avance légèrement std::cout << "GoTo 250, Y actuelle -> -Pi/2" << std::endl; goTo (250, motor_.getY (), -M_PI / 2); // TODO // Faire un fuck the wall en Y // On met la caméra // On avance d'une valeur fixe si on a rien // Si on a quelque chose, on y va // On remonte, rotation de 0 puis M_PI / 2 // On pose, on recule de 100, on baisse l'ascenceur // On pivote à - M_PI / 2 // On réitère si on est pas au bout 1800 // On recommence l'algo dans l'autre sens }