From bf03b37b06877b55e4f23b627647a6b156ed664e Mon Sep 17 00:00:00 2001 From: haller Date: Thu, 25 May 2006 22:14:43 +0000 Subject: * Constification de getFd dans la classe Serial et ses clients * Fonction afterMatch dans ai * Fonction motorSmartFindHole * Déclaration de la fonction motorDeblock * Ajout d'info sur le lcd lors de l'init * Utilisation des capteurs barillet --- i/marvin/src/ai/ai.cc | 102 +++++++++++++++++++++++++++++++++++++++++++-- i/marvin/src/ai/ai.hh | 8 +++- i/marvin/src/ai/test_ai.cc | 5 +++ 3 files changed, 111 insertions(+), 4 deletions(-) (limited to 'i/marvin/src/ai') diff --git a/i/marvin/src/ai/ai.cc b/i/marvin/src/ai/ai.cc index 53f191e..0d24afa 100644 --- a/i/marvin/src/ai/ai.cc +++ b/i/marvin/src/ai/ai.cc @@ -132,6 +132,10 @@ Ai::waitJack (bool out) void Ai::prepare (void) { + es_.lcdPrint(""); + 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 @@ -139,7 +143,11 @@ Ai::prepare (void) waitJack (false); // We reference all the color referenceSensors (); + // Set actual position + motor_.getAsserv().setPos(0,0,0); es_.barilletInit(); + es_.lcdPrint("Virer le jack..."); + es_.lcdPrint("PRET POUR MATCH"); while (!update ()); // We first wait for the jack to be put inside waitJack (true); @@ -152,6 +160,27 @@ Ai::prepare (void) 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) @@ -188,7 +217,7 @@ void Ai::progHomoloRobal(void) motorMove(500, 0); // temporisation (100); std::cout << "Find hole !" << std::endl; - motorFindHole (); + motorBasicFindHole (); /// On tourne XXX /// motorRotate(-M_PI/4); @@ -214,7 +243,7 @@ void Ai::progHomoloRobal(void) motorMove(300, 0); std::cout << "Find hole !" << std::endl; - motorFindHole (); + motorBasicFindHole (); es_.dropWhiteBall(); while (!update ()); temporisation (3000); @@ -225,6 +254,15 @@ void Ai::progHomoloRobal(void) /// Tourner 3 fois en chantant du Mickael Jackson /// Again.... /// Fin du match + afterMatch(); +} + +void +Ai::progMatch(void) +{ + /// On prépare le robot + prepare(); + /// Faire des crêpes... } void @@ -250,7 +288,7 @@ Ai::motorRotate (double d) } void -Ai::motorFindHole (void) +Ai::motorBasicFindHole (void) { motor_.findHole (); do @@ -259,3 +297,61 @@ Ai::motorFindHole (void) } while (!motor_.finish ()); } + +/// 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(); + /// 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 + motorRotate(-0.21); + motor_.findHole(); + } + } + while (!motor_.finish() || motor_.blocked()); + /// On regarde si on a blocké + if(motor_.blocked()) + return -1; + if(motor_.finish()) + return es_.colorSeen(es_.holeRVB_); + /// XXX Le return qui ne doit jamais arriver normalement... + return 0; +} + +/// Procedure to unblock the robal +void +Ai::motorDeblock (void) +{ +} diff --git a/i/marvin/src/ai/ai.hh b/i/marvin/src/ai/ai.hh index f0143ec..b7b0833 100644 --- a/i/marvin/src/ai/ai.hh +++ b/i/marvin/src/ai/ai.hh @@ -68,12 +68,18 @@ class Ai bool sync (void); /// Init things for a match. void prepare (void); + /// Some after after a match + void afterMatch(void); /// Reference sensors void referenceSensors (void); /// programme d'homologation du robal void progHomoloRobal(void); + /// programme de match du robal + void progMatch(void); void motorMove (int d, int a); void motorRotate (double d); - void motorFindHole (void); + void motorBasicFindHole (void); + int motorSmartFindHole (double distOut); + void motorDeblock(void); }; #endif // ai_hh diff --git a/i/marvin/src/ai/test_ai.cc b/i/marvin/src/ai/test_ai.cc index 27d32a1..dd51b7d 100644 --- a/i/marvin/src/ai/test_ai.cc +++ b/i/marvin/src/ai/test_ai.cc @@ -61,6 +61,9 @@ class TestAi : public Tester "Initialise le robal(void)"); interpreter.add("prepare", Interpreter::memFunc(ai_, &Ai::prepare), "Lance la procédure pré-Match"); + interpreter.add("afterMatch", Interpreter::memFunc(ai_, + &Ai::afterMatch), + "Lance la procédure post match"); interpreter.add("stop", Interpreter::memFunc(ai_, &Ai::stop), "Stop le robal(void)"); interpreter.add("tempo", Interpreter::memFunc(ai_, @@ -74,6 +77,8 @@ class TestAi : public Tester interpreter.add("homologation", Interpreter::memFunc(ai_, &Ai::progHomoloRobal), "Programme d'homologation du robal"); + interpreter.add("match", Interpreter::memFunc(ai_, &Ai::progMatch), + "Programme de match qui déchire du robal"); interpreter.add ("_postcall", Interpreter::memFunc (*this, &TestAi::postcall)); } -- cgit v1.2.3