From 8f486613be58ced269db1d437e560c16558604e8 Mon Sep 17 00:00:00 2001 From: becquet Date: Thu, 10 May 2007 18:49:20 +0000 Subject: Création de chuck, le programme du robot 2007. --- i/chuck/src/ai/ai.cc | 511 +++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 511 insertions(+) create mode 100644 i/chuck/src/ai/ai.cc (limited to 'i/chuck/src/ai/ai.cc') diff --git a/i/chuck/src/ai/ai.cc b/i/chuck/src/ai/ai.cc new file mode 100644 index 0000000..c9be04a --- /dev/null +++ b/i/chuck/src/ai/ai.cc @@ -0,0 +1,511 @@ +// AI.CC +// marvin - programme du robot 2006. {{{ +// +// Copyright (C) 2006 Nicolas Haller +// +// 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. +// +// Contact : +// Web: http://perso.efrei.fr/~haller/ +// Email: +// }}} +#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) +{ +} -- cgit v1.2.3