// ai.cc // chuck - programme du robot 2007 {{{ // // Copyright (C) 2007 Romain Becquet // // Robot APB Team/Efrei 2007. // Web: http://assos.efrei.fr/si2e/ // Email: si2e 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 "ai.hh" #include "config/config.hh" #include "timer/timer.hh" #include /// Fonction callback useless static void callback (void) {} //constructeur Ai::Ai (const Config & config) :es_(config), log_ ("Ai"), schedulableMotor_(callback, motor_.getFd()), schedulableEs_(callback, es_.getFd()), round_duration_ (config.get ("ai.round_duration")), schedule_time_ (config.get ("ai.schedule_time")), reference_sensor_mask_ (config.get ("ai.ref_sensor_mask")) { scheduler_.insert (schedulableMotor_); scheduler_.insert (schedulableEs_); } Ai::~Ai(void) { // On réinitialise // Initialise les moteurs // XXX motor_.reset (); // initialise la carte es es_.reset (); // On sync do { scheduler_.schedule (500, true); } while (!sync ()); } // Initialisation du robal void Ai::init (void) { // Initialisation de motor motor_.reset (); // Initialisation de la carte Es es_.reset (); // Paf on sync while (!update ()); } /// Stop the mouvement of the motor void Ai::stop (void) { motor_.stop (); // XXX In fact, we should wait a little here... while (!update ()); } /// Synchronize data of under class bool Ai::sync (void) { bool motor = motor_.sync (); bool es = es_.sync (); return es && motor; } /// Wait for something to happened bool Ai::update (void) { /// Wait schedule_time scheduler_.schedule (schedule_time_, true); bool retour = sync (); // On vérifie que le match n'est pas fini if (Timer::getRoundTime () > round_duration_) throw std::runtime_error ("End of match ! You win !"); return retour; } /// Function to wait a little but still syncing data void Ai::temporisation(int msec) { // Get current time of process int time = Timer::getProgramTime(); time += msec; // Update until we have spend enough time do { update(); } while (time > Timer::getProgramTime()); } /// Wait until jack is out (true) or in (false) void Ai::waitJack (bool out) { do { update (); } while (es_.isJackOut () != out); } /// Init things for a match. void Ai::prepare (void) { es_.lcdPrint(""); es_.lcdPrint("Prepa match..."); es_.lcdPrint("Inserer Jack"); es_.setOthersStat (10); while (!update ()); // XXX We should check if the jack is not already in // We first wait for the jack to be put inside waitJack (false); // We reference all the color referenceSensors (); es_.barilletInit(); es_.lcdPrint("Barillet pret ?"); es_.lcdPrint("Virer le jack !"); while (!update ()); // We first wait for the jack to be put inside waitJack (true); // Set actual position motor_.getAsserv().setPos(0,0,0); // Ok the match begin ! Go go go ! Timer::startRound (); es_.setOthersStat (0); // Shut up fucking beach ! es_.setOthersStat (0); es_.setOthersStat (0); while (!update ()); } /// Some after after a match void Ai::afterMatch(void) { // On attend de mettre le jack après le match es_.lcdPrint("FIN DU MATCH"); // es_.lcdPrint("Inserer jack..."); while (!update()); waitJack(false); // On vide le barillet es_.barilletEmpty(); es_.lcdPrint("Vidange barillet"); while (!update()); waitJack(true); // On init les cartes histoire de es_.reset(); motor_.reset(); es_.lcdPrint("Power off!!"); while (!update()); } /// Reference sensors void Ai::referenceSensors (void) { // Reference sensors es_.refColor (reference_sensor_mask_); // Update data while (!update ()); // Disable capturing all sensors es_.enableAllSensors (false); // Update data while (!update ()); } /// programme d'homologation du robal void Ai::progHomoloRobal(void) { /// On init le robal prepare(); es_.barilletDebutLancement(); while (!update ()); temporisation (1000); es_.barilletLancement(); while (!update ()); /// on avance un poil parce que le mur c'est mal std::cout << "Move !" << std::endl; motorMove(500, 0); std::cout << "Rotate !" << std::endl; motorRotate(-0.54); std::cout << "Move !" << std::endl; motorMove(1200, 0); std::cout << "Rotate !" << std::endl; motorRotate(1.40); motorMove(500, 0); // temporisation (100); std::cout << "Find hole !" << std::endl; motorBasicFindHole (); /// On tourne XXX /// motorRotate(-M_PI/4); /// On cherche au moins une balle (sinon c'est con) /// motorMove(1000, 0); /// XXX Voir la distance /// XXX Avancer d'une certaine facon pour choper des balles /// XXX Ouais ba y'a encore ddes truc à faire là /// On cherche un tru (repositionnement??) /// motor_.lockGoodHole(); /// while (!update ()); /// motor_.findHole(); /// while (!update ()); /// On trouve un trou, chouette /// Aspirer le trou //es_.extraitBalle(); /// mettre une baballe es_.dropWhiteBall(); while (!update ()); temporisation (3000); es_.deposeBalle (); while (!update ()); temporisation (3000); motorMove(300, 0); std::cout << "Find hole !" << std::endl; motorBasicFindHole (); es_.dropWhiteBall(); while (!update ()); temporisation (3000); es_.deposeBalle (); while (!update ()); temporisation (3000); motorMove(300, 0); /// Tourner 3 fois en chantant du Mickael Jackson /// Again.... /// Fin du match afterMatch(); } void Ai::progMatch(void) { double dummy, aStart, aEnd; // Init of cards init (); /// On prépare le robot prepare(); /// Faire des crêpes... /// Init du barillet es_.barilletDebutLancement(); while (!update ()); log_ ("match") << "Totem activator"; es_.totemActivator (true); /// En avant guingamp log_ ("match") << "Eloignement du bord"; motorMove(480,0); es_.barilletLancement(); while (!update ()); /// On tourner de -90° log_ ("match") << "Petite rotation de 90°"; motorMove(216,-216); /// On dit bonjour au mur log_ ("match") << "Va à l'opposé"; if (!motorMove (1000, 0)) { log_ ("match") << "Un mur ! Reculons"; motorMove (-60, 0); } /// On tourne bizarrement mais de 90° log_ ("match") << "On tourne à 90° en deux fois"; motorMove(-36,36); motorMove(180, 180); /// On avance un peu log_ ("match") << "On avance de l'autre coté"; motorMove (1420, 0); log_ ("match") << "On recule un peu"; motorMove (-150, 0); log_ ("match") << "On tourne"; motorMove (216, 216); motorMove (-150, 0); /* Here, we are in front of all our holes */ int rotationSens = 1; const double distanceMax[2] = { -1600., 200. }; // Force a turn back ! double distanceCur, remainingDist; int ret; /* Save angle */ motor_.getPosition (dummy, dummy, aStart); do { log_ ("match") << "Start loop for finding holes"; // Save Y position each time motor_.getPosition (dummy, distanceCur, dummy); // Look for an hole ! if (rotationSens % 2) remainingDist = std::abs (distanceMax[1] - distanceCur); else remainingDist = std::abs (distanceMax[0] - distanceCur); // Protection again stupid computing if (remainingDist > 0.) ret = motorSmartFindHole (remainingDist); else // Force a turn back ! ret = 0; switch (ret) { case 0: // Here we should turn back in the other way log_ ("match") << "Trop long, demi tour !"; if (rotationSens % 2) motor_.move (432, -432); else motor_.move (432, 432); rotationSens++; /* Compute the new angle */ motor_.getPosition (dummy, dummy, aStart); break; case -2: /// La couleur est unknown log_ ("match") << "Hooooo une couleur de l'espace!!!!"; // break; case 2: case 1: { log_ ("match") << "Ok on a une couleur!"; bool retDrop; //if (((ret == es_.blueColor_) && es_.isColorModeBlue () ) // || ((ret != es_.blueColor_) && !es_.isColorModeBlue ())) if (true) { // If it is our colour, put a white ball, log_ ("match") << "C'est notre couleur -> drop white !"; retDrop = es_.dropWhiteBall(); while (!update ()); if (retDrop) { log_ ("match") << "Tempo & depot!"; temporisation (3000); es_.deposeBalle (); temporisation (1000); while (!update ()); } } else { // otherwise, put a black one. log_ ("match") << "Pas notre couleur, drop noir!"; retDrop = es_.dropBlackBall(); while (!update ()); if (retDrop) { log_ ("match") << "Tempo & depot!"; temporisation (3000); es_.deposeBalle (); temporisation (1000); while (!update ()); } } // Put ourself back in the right direction motor_.getPosition (dummy, dummy, aEnd); log_ ("match") << "Remise en place de l'angle : " << aStart << " end " << aEnd; motor_.rotate (aStart - aEnd); } break; default: log_ ("match") << "Hooooo on a default!!!!"; break; } } while (true); afterMatch(); } /// Avance le robal et dit si il a finit (true) ou si il a bloqué bool Ai::motorMove (int d, int a) { motor_.move(d, a); /// XXX Voir la distance do { update (); } while (!motor_.finish ()/* && !motor_.blocked()*/); if(motor_.finish()) return true; return false; } void Ai::motorRotate (double d) { motor_.rotate (d); do { update (); } while (!motor_.finish ()); } void Ai::motorBasicFindHole (void) { motor_.findHole (); do { update (); } while (!motor_.finish ()); es_.enableAllSensors (true); es_.setRVBHoleStat (2); temporisation (500); log_ ("FindHole") << "We see" << es_.colorSeen (es_.holeRVB_) ; es_.setRVBHoleStat (0); } /// Renvoie une couleur définie dans es ou -1 si on s'est mangé un mur ou 0 si /// distanceOut int Ai::motorSmartFindHole (double distOut) { /// On sauvegarde la position actuelle double bx, x, by, y, ba, a; double dist; motor_.getPosition(bx, by, ba); /// on lance le robal à la recherche d'un tru motor_.findHole(); do { update(); if (distOut != 0) { /// On regarde si on a pas atteint la distOut motor_.getPosition(x, y, a); /// XXX C'est propre ça? dist = sqrt(((x - bx)*(x - bx)) + ((y - by)*(y - by))); if (dist > distOut) return 0; } /// on regarde si les capteurs voit un trou if(es_.colorSeen(es_.leftFrontRVB_) == es_.redColor_ || es_.colorSeen(es_.leftFrontRVB_) == es_.blueColor_) { // Si c'est le cas on repositionne le robot motor_.stop(); while(!update()); // XXX Vérifier l'angle motorRotate(0.21); motor_.findHole(); } else if(es_.colorSeen(es_.rightFrontRVB_) == es_.redColor_ || es_.colorSeen(es_.rightFrontRVB_) == es_.blueColor_) { // Si c'est le cas on repositionne le robot motor_.stop(); while(!update()); // XXX Vérifier l'angle log_ ("SmartFindHole") << "Rotation spéciale ho un trou sous le capteur"; motorRotate(-0.21); motor_.findHole(); } } while (!motor_.finish() /*&& !motor_.blocked()*/); /// On regarde si on a blocké // if(motor_.blocked()) // return -1; if(motor_.finish()) { es_.enableAllSensors (true); es_.setRVBHoleStat (2); temporisation (500); int ret = es_.colorSeen(es_.holeRVB_); es_.setRVBHoleStat (0); es_.enableAllSensors (false); return ret; } /// XXX Le return qui ne doit jamais arriver normalement... return 0; } /// Procedure to unblock the robal void Ai::motorDeblock (void) { }