// 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")) { scheduler_.insert (schedulableMotor_); scheduler_.insert (schedulableEs_); } /// Destructeur Ai::~Ai (void) { // On réinitialise init (); // On sync do { update (); } while (motor_.sync ()); } /// Initialise le robot void Ai::init(void) { // Initialise les moteurs motor_.init(); // initialise la carte es es_.init(); // on init la vision //oVision_.init(motor_.colorState() ? Group::redSkittle : // Group::greenSkittle); /// XXX while (!update()); } /// stop le robot void Ai::stop(void) { // Stop les moteurs motor_.stop(); // On sync do { update (); } while (!motor_.idle ()); } /// Lance le robot void Ai::run(void) { // Lancement du placement pour les matchs initMatch (); /*** AI d'homologation ***/ /* // On temporise pour laisser Taz sortir std::cout << "Attente du 15 secondes" << std::endl; temporisation(15000); // On avance pour pouvoir aller vers la boule parallèlement au socle std::cout << "On avance de 540mm" << std::endl; basic(540); // 600 - 60 // Aller devant la boule std::cout << "GoTo 870,-675 -> Pi" << std::endl; goTo(870, -675, M_PI); // On change l'accélération motor_.setAccel(32); // Taper la boule std::cout << "Recule de 410mm" << std::endl; basic(-410); // On restaure la valeur de l'acceleration motor_.setAccel(64); // se placer devant nos quilles près du pont std::cout << "Avance de 410mm" << std::endl; basic(410); // On va derière les quilles std::cout << "GoTo" << std::endl; goTo(motor_.getX(),motor_.getY() - 150 ,M_PI); // On set la vitesse à 2 int MaxLSpeedConf = motor_.getMaxLSpeed(); std::cout << "Reduction vitesse" << std::endl; motor_.setMaxSpeed(2, -1); // On colle les quilles std::cout << "Recule de 400mm" << std::endl; basic (-400); // 260 - 10 de marge // On retaure la vitesse std::cout << "Acceleration vitesse" << std::endl; motor_.setMaxSpeed(MaxLSpeedConf, -1); // XXX On temporise et on fait mumuse avec l'ascenceur */ /*** AI du 1er match ***/ temporisation (30000); basic ( 600 ); rotation ( - M_PI / 2 ); basic ( 300 ); rotation ( M_PI ); basic ( 450 ); // On a fini notre run std::cout << "On a fini" << std::endl; } // Balistique non fiable. void Ai::initBalNonFiable (void) { // Lancement du placement pour les matchs initMatch (); // Protection des quilles devant le pont protectQuillePont (); } // 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 (); } // test void Ai::initTest (void) { /*** Init du match ***/ init(); // Reset la PWM à zéro si on a fait de la merde en le posant sur la table resetPwm (); 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; // On lance le timer de début de match std::cout << "Lancmeent du match" << std::endl; Timer::startRound(); /*** Fin de l'init du match ***/ parcourtFond (); } // 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 170" << std::endl; goTo (200, motor_.getY (), - M_PI / 2); std::cout << "On avance de 30 cm" << std::endl; basic (300); /*** Fin du préplacement ***/ /*** Boucle de parcours de la ligne de fond ***/ while (motor_.getY () > -1700) { std::cout << "On lève" << std::endl; ascenceur (true); std::cout << "Vitesse lente de rotation 2" << std::endl; setMaxSpeed (-1, 2); std::cout << "Tourne un peu" << std::endl; rotation (M_PI); std::cout << "Fini le tour" << std::endl; rotation (M_PI / 2); std::cout << "On attend" << std::endl; temporisation (1000); std::cout << "On lache" << std::endl; ventouses (); std::cout << "On attend" << std::endl; temporisation (1500); std::cout << "On restaure les vitesses" << std::endl; setMaxSpeed (-1, 4); std::cout << "On recule de 100" << 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 100" << std::endl; basic (200); } std::cout << "Fini" << std::endl; } // Balistique fiable. void Ai::initBalFiable (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 } // 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 870,-675 -> Pi" << std::endl; goTo(1225, -1125, M_PI); // On change la vitesse max pour reculer std::cout << "On change la vitesse à 2" << std::endl; motor_.setMaxSpeed (2, -1); // Taper la boule std::cout << "On recule lentement de 200mm" << std::endl; basic(-200); // On restaure la vitesse std::cout << "On restaure la vitesse à 8" << std::endl; motor_.setMaxSpeed (8, -1); // On attend un peu pour protéger std::cout << "On temporise" << std::endl; temporisation (1500); // 15000 XXX A changer // XXX à finir // On se barre des quilles basic (200); // On retourne à l'origine goTo (450, -300, M_PI); // On avance légèrement goTo (250, motor_.getY (), -M_PI / 2); // XXX 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 } /// Attend. void Ai::wait (int t) { motor_.wait(t); } /// Fonction de temporisation void Ai::temporisation(int t) { int time = Timer::getRoundTime(); time += t; while (time > Timer::getRoundTime()) { update(); } } // 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 XXX 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) /// XXX Temps à régler dans la config { es_.ventouses(); while (!update()); } /// Attend une mise à jour bool Ai::update (void) { scheduler_.schedule(500, true); // Gestion des cas critiques /// XXX // 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 sync (); } /// 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()); } /// 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 du 15 secondes" << std::endl; temporisation(15000); } void Ai::setMaxSpeed (int l, int r) { motor_.setMaxSpeed (l, r); while (!update ()); }