From 6816e1505503bfeb39b3f9e4156e2514907cdbba Mon Sep 17 00:00:00 2001 From: dufourj Date: Fri, 26 May 2006 15:47:50 +0000 Subject: Génral : - commit post 3eme match. --- i/marvin/runtime/rc/config | 1 + i/marvin/src/ai/ai.cc | 129 ++++++++++++++++++++++++++++++++++++--------- i/marvin/src/ai/ai.hh | 1 + i/marvin/src/es/es.cc | 11 +++- 4 files changed, 117 insertions(+), 25 deletions(-) diff --git a/i/marvin/runtime/rc/config b/i/marvin/runtime/rc/config index da19c8b..9b977a8 100644 --- a/i/marvin/runtime/rc/config +++ b/i/marvin/runtime/rc/config @@ -89,6 +89,7 @@ ai.ref_sensor_mask = 127 # log.logger.asserv = "null" log.logger.default = "stdout" log.level.default = "debug" +log.logger.proto = "null" ### Exemples foo = "toto" diff --git a/i/marvin/src/ai/ai.cc b/i/marvin/src/ai/ai.cc index 19f3cca..373fe2a 100644 --- a/i/marvin/src/ai/ai.cc +++ b/i/marvin/src/ai/ai.cc @@ -142,14 +142,14 @@ Ai::prepare (void) waitJack (false); // We reference all the color referenceSensors (); - // Set actual position - motor_.getAsserv().setPos(0,0,0); 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); @@ -259,6 +259,7 @@ void Ai::progHomoloRobal(void) void Ai::progMatch(void) { + double dummy, aStart, aEnd; init (); // bool isFinish; /// On prépare le robot @@ -283,13 +284,13 @@ Ai::progMatch(void) if (!motorMove (1000, 0)) { log_ ("match") << "Un mur ! Reculons"; - motorMove (-70, 0); + motorMove (-60, 0); } // motorMove(200, 0); /// On tourne bizarrement mais de 90° log_ ("match") << "On tourne à 90° en deux fois"; motorMove(-36,36); - motorMove(190, 190); + motorMove(180, 180); /// On avance un peu // log_ ("match") << "On recule un peu"; // motorMove (-110, 0); @@ -303,23 +304,96 @@ Ai::progMatch(void) // motorMove (800, 0); // motorBasicFindHole (); log_ ("match") << "On avance de l'autre coté"; - motorMove (1320, 0); + motorMove (1420, 0); log_ ("match") << "On recule un peu"; - motorMove (-420, 0); + motorMove (-150, 0); log_ ("match") << "On tourne"; - motorMove (150, 150); - motorBasicFindHole (); - es_.dropWhiteBall(); - while (!update ()); - temporisation (3000); - es_.deposeBalle (); - while (!update ()); - motorBasicFindHole (); - es_.dropWhiteBall(); - while (!update ()); - temporisation (3000); - es_.deposeBalle (); - + motorMove (216, 216); + motorMove (-150, 0); + + /* Here, we are in front of all our holes */ + + int rotationSens = 1; + const double distanceMax[2] = { -1600, -200 }; + double distanceCur; + + motor_.getPosition (dummy, distanceCur, aStart); + do + { + // TODO compute remaining value + // alors une petite blague : Comment les alsacien aime-t-il les oeufs + // ? + // réponse : Hopla !!!! (Tb) :-) + log_ ("match") << "Start loop for find hole"; + + + motor_.getPosition (dummy, distanceCur, dummy); + + int ret = motorSmartFindHole (distanceCur - distanceMax[rotationSens % 2]); + switch (ret) + { + case 0: + log_ ("match") << "Trop long, demi tour !"; + if (rotationSens % 2) + motor_.move (432, -432); + else + motor_.move (432, 432); + rotationSens++; + // Distance dépassé + // Here we should turn back in the other way + 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) + { + log_ ("match") << "C'est notre couleur!"; + // If it is our colour, put a white ball, + log_ ("match") << "Drop blanc!"; + retDrop = es_.dropWhiteBall(); + while (!update ()); + if (retDrop) + { + log_ ("match") << "Tempo & depot!"; + temporisation (3000); + es_.deposeBalle (); + while (!update ()); + } + } + else + { + log_ ("match") << "Drop noir!"; + retDrop = es_.dropBlackBall(); + while (!update ()); + if (retDrop) + { + log_ ("match") << "Tempo & depot!"; + temporisation (3000); + es_.deposeBalle (); + while (!update ()); + // otherwise, put a black one. + } + } + } + break; + default: + log_ ("match") << "Hooooo on a default!!!!"; + break; + } + motor_.getPosition (dummy, dummy, aEnd); + log_ ("match") << "Remise en place de l'angle! " << aStart - aEnd; + motor_.rotate (aStart - aEnd); + motor_.getPosition (dummy, dummy, aStart); + } while (true); + afterMatch(); } @@ -332,7 +406,7 @@ Ai::motorMove (int d, int a) { update (); } - while (!motor_.finish () && !motor_.blocked()); + while (!motor_.finish ()/* && !motor_.blocked()*/); if(motor_.finish()) return true; return false; @@ -401,16 +475,23 @@ Ai::motorSmartFindHole (double distOut) 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()); + while (!motor_.finish() /*&& !motor_.blocked()*/); /// On regarde si on a blocké - if(motor_.blocked()) - return -1; +// if(motor_.blocked()) +// return -1; if(motor_.finish()) - return es_.colorSeen(es_.holeRVB_); + { + es_.enableAllSensors (true); + temporisation (100); + int ret = es_.colorSeen(es_.holeRVB_); + es_.enableAllSensors (false); + return ret; + } /// XXX Le return qui ne doit jamais arriver normalement... return 0; } diff --git a/i/marvin/src/ai/ai.hh b/i/marvin/src/ai/ai.hh index e3f7e19..29598b4 100644 --- a/i/marvin/src/ai/ai.hh +++ b/i/marvin/src/ai/ai.hh @@ -48,6 +48,7 @@ class Ai const int round_duration_; const int schedule_time_; const int reference_sensor_mask_; + bool mustRotate_; public: /// Constructeur Ai(const Config & config); diff --git a/i/marvin/src/es/es.cc b/i/marvin/src/es/es.cc index 7b0505b..03fa74b 100644 --- a/i/marvin/src/es/es.cc +++ b/i/marvin/src/es/es.cc @@ -32,7 +32,9 @@ /// Constructeur Es::Es (const Config & config) : proto_ (*this), log_ ("Es"), lcdKeyPressed_ (-1), front_sensor_ (false), - jackIn_ (false), colorModeBlue_ (false),positionBarillet_(avant0),frontTurbineIsFull_(false), unknownColor_ (-1), redColor_ (2), blueColor_ (1), greenColor_ (0), whiteColor_ (5), + jackIn_ (false), colorModeBlue_ +(false),positionBarillet_(avant0),frontTurbineIsFull_(false), unknownColor_ +(-2), redColor_ (2), blueColor_ (1), greenColor_ (0), whiteColor_ (5), blackColor_ (4), leftFrontRVB_ (0), rightFrontRVB_ (1), holeRVB_ (2), frontBallRVB_ (3), rearBallRVB_ (4) { @@ -314,6 +316,7 @@ Es::deposeBalle(void) barilletDebutLancement(); log_ ("Es::Barillet", Log::debug) << "DROP!! "; frontTurbineIsFull_ = false; + barilletLancement(); } /// Dépose une balle blanche @@ -491,6 +494,10 @@ void Es::receive(char command, const Proto::Frame & frame) case 3: // une balle miam par l'arrière newBallRear(); break; + case 4: + // A ball has been by the sensor at the front of the barillet + enableAllSensors (true); + break; } shutUp (); } @@ -649,6 +656,7 @@ Es::newBallFront(void) if(!barilletIsFull()) barilletLancement(); log_ ("Es::Barillet", Log::debug) << "gobage balle par l'avant."; + enableAllSensors (false); } /// Analyse une balle arrivant par derrière @@ -687,6 +695,7 @@ Es::newBallRear(void) break; } log_ ("Es::Barillet", Log::debug) << "gobage balle par l'arriere."; + enableAllSensors (false); } /// Update system for one sensor -- cgit v1.2.3