From 4777e2a1e5211f015d760c1e8614d3936d6b9045 Mon Sep 17 00:00:00 2001 From: haller Date: Thu, 25 May 2006 01:53:31 +0000 Subject: * Gestion un peu plus avancé du barillet --- i/marvin/src/ai/ai.cc | 5 +- i/marvin/src/es/es.cc | 216 +++++++++++++++++++++++++++++++++++++++++++- i/marvin/src/es/es.hh | 25 ++++- i/marvin/src/es/test_es.cc | 9 ++ i/marvin/src/motor/motor.cc | 20 ++++ i/marvin/src/motor/motor.hh | 4 + 6 files changed, 271 insertions(+), 8 deletions(-) (limited to 'i/marvin') diff --git a/i/marvin/src/ai/ai.cc b/i/marvin/src/ai/ai.cc index bbf3158..d9bfd0a 100644 --- a/i/marvin/src/ai/ai.cc +++ b/i/marvin/src/ai/ai.cc @@ -168,16 +168,17 @@ void Ai::progHomoloRobal(void) motor_.rotate(-M_PI/4); /// On cherche au moins une balle (sinon c'est con) es_.barilletLancement(); + motor_.move(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??) - // XXX motor_.lockGoodHole(); + motor_.lockGoodHole(); motor_.findHole(); /// On trouve un trou, chouette /// Aspirer le trou es_.extraitBalle(); /// mettre une baballe - // XXX es_.deposeBlanche(); + es_.dropWhiteBall(); /// Tourner 3 fois en chantant du Mickael Jackson /// Again.... /// Fin du match diff --git a/i/marvin/src/es/es.cc b/i/marvin/src/es/es.cc index f26cf33..534b136 100644 --- a/i/marvin/src/es/es.cc +++ b/i/marvin/src/es/es.cc @@ -31,8 +31,8 @@ /// Constructeur Es::Es (const Config & config) - : proto_ (*this), log_ ("Es"), lcdKeyPressed_ (-1), - front_sensor_ (false), jackIn_ (false), colorModeBlue_ (false), + : proto_ (*this), log_ ("Es"), lcdKeyPressed_ (-1), front_sensor_ (false), + jackIn_ (false), colorModeBlue_ (false),frontTurbineIsFull_(false), unknownColor_ (-1), redColor_ (2), blueColor_ (1), greenColor_ (0), whiteColor_ (5), blackColor_ (4), leftFrontRVB_ (0), rightFrontRVB_ (1), holeRVB_ (2), frontBallRVB_ (3), rearBallRVB_ (4) @@ -42,6 +42,9 @@ Es::Es (const Config & config) proto_.open (tty_); sharps_.resize (3); + // Normalement, le barillet est vide... + for(int i = 0; i < 5 ; i++) + stockBarillet[i] = empty; for (int compt = 0; compt < 5; compt++) { @@ -115,6 +118,7 @@ Es::reset (void) lcdGetKey (lcdKeyStat_); enableAllSensors (true); + log_ ("Es", Log::debug) << "Reset Es done."; } /// Stat for the main () @@ -278,18 +282,22 @@ void Es::barilletInit (void) { proto_.send ('i'); + frontTurbineIsFull_ = false; } /// Init turbine to minimal speed void Es::barilletDebutLancement (void) { proto_.send ('g'); + frontTurbineIsFull_ = false; } /// Init front turbine to full speed void Es::barilletLancement (void) { proto_.send ('h'); + frontTurbineIsFull_ = true; + log_ ("Es::Barillet", Log::debug) << "Lancement turbine avant."; } /// Put barillet in sleep mode @@ -297,30 +305,144 @@ void Es::barilletSleep (void) { proto_.send ('s'); + frontTurbineIsFull_ = false; } // Dépose une balle du barillet -void Es::deposeBalle(void) +void +Es::deposeBalle(void) { proto_.send ('d'); + frontTurbineIsFull_ = false; +} + +/// Dépose une balle blanche +bool +Es::dropWhiteBall(void) +{ + for(int i = 0; i < 5; i++) + if(stockBarillet[i] == white) + { + switch (i) + { + case 0: + rotationBarillet(arriere0); + positionBarillet_ = arriere0; + break; + case 1: + rotationBarillet(arriere1); + positionBarillet_ = arriere1; + break; + case 2: + rotationBarillet(arriere2); + positionBarillet_ = arriere2; + break; + case 3: + rotationBarillet(arriere3); + positionBarillet_ = arriere3; + break; + case 4: + rotationBarillet(arriere4); + positionBarillet_ = arriere4; + break; + } + return true; + } + return false; +} + +/// Dépose une balle noire +bool +Es::dropBlackBall(void) +{ + for(int i = 0; i < 5; i++) + if(stockBarillet[i] == black) + { + switch (i) + { + case 0: + rotationBarillet(arriere0); + positionBarillet_ = arriere0; + break; + case 1: + rotationBarillet(arriere1); + positionBarillet_ = arriere1; + break; + case 2: + rotationBarillet(arriere2); + positionBarillet_ = arriere2; + break; + case 3: + rotationBarillet(arriere3); + positionBarillet_ = arriere3; + break; + case 4: + rotationBarillet(arriere4); + positionBarillet_ = arriere4; + break; + } + return true; + } + return false; } +/// Place un trou vide à l'avant +bool +Es::setEmptyHoleFront(void) +{ + if(barilletIsFull()) + return false; + for(int i = 0; i < 5; i++) + if(stockBarillet[i] == empty) + { + switch (i) + { + case 0: + rotationBarillet(avant0); + positionBarillet_ = avant0; + break; + case 1: + rotationBarillet(avant1); + positionBarillet_ = avant1; + break; + case 2: + rotationBarillet(avant2); + positionBarillet_ = avant2; + break; + case 3: + rotationBarillet(avant3); + positionBarillet_ = avant3; + break; + case 4: + rotationBarillet(avant4); + positionBarillet_ = avant4; + break; + } + return true; + } + return false; +} /// Extrait une balle -void Es::extraitBalle(void) +void +Es::extraitBalle(void) { proto_.send ('e'); + frontTurbineIsFull_ = false; + log_ ("Es::Barillet", Log::debug) << "Extraction baballe."; } /// Rotation du barillet void Es::rotationBarillet(int posFinal) { proto_.send ('t', "b", posFinal); + frontTurbineIsFull_ = false; } /// Empty everything in the barillet void Es::barilletEmpty (void) { proto_.send ('n'); + frontTurbineIsFull_ = false; } void Es::receive(char command, const Proto::Frame & frame) @@ -348,6 +470,12 @@ void Es::receive(char command, const Proto::Frame & frame) case 1: front_sensor_ = true; break; + case 2: // une balle miam par l'avant + newBallFront(); + break; + case 3: // une balle miam par l'arrière + newBallRear(); + break; } shutUp (); } @@ -464,6 +592,77 @@ Es::decodeColor (int color) } } +/// Analyse une balle arrivant par devant +void +Es::newBallFront(void) +{ + /// La turbine est off + frontTurbineIsFull_ = false; + /// On analyse la baballe + switch (positionBarillet_) + { + case avant0: + stockBarillet[0] = colorSeen(frontBallRVB_)==whiteColor_?white:black; + positionBarillet_ = avant4; + break; + case avant1: + stockBarillet[1] = colorSeen(frontBallRVB_)==whiteColor_?white:black; + positionBarillet_ = avant0; + break; + case avant2: + stockBarillet[2] = colorSeen(frontBallRVB_)==whiteColor_?white:black; + positionBarillet_ = avant1; + break; + case avant3: + stockBarillet[3] = colorSeen(frontBallRVB_)==whiteColor_?white:black; + positionBarillet_ = avant2; + break; + case avant4: + stockBarillet[4] = colorSeen(frontBallRVB_)==whiteColor_?white:black; + positionBarillet_ = avant3; + break; + default: + break; + } + /// On regarde si on redémarre la turbine + if(!barilletIsFull()) + barilletLancement(); + log_ ("Es::Barillet", Log::debug) << "gobage balle par l'avant."; +} + +/// Analyse une balle arrivant par derrière +void +Es::newBallRear(void) +{ + /// On analyse la baballe + switch (positionBarillet_) + { + case arriere0: + stockBarillet[0] = colorSeen(frontBallRVB_)==whiteColor_?white:black; + positionBarillet_ = arriere4; + break; + case arriere1: + stockBarillet[1] = colorSeen(frontBallRVB_)==whiteColor_?white:black; + positionBarillet_ = arriere0; + break; + case arriere2: + stockBarillet[2] = colorSeen(frontBallRVB_)==whiteColor_?white:black; + positionBarillet_ = arriere1; + break; + case arriere3: + stockBarillet[3] = colorSeen(frontBallRVB_)==whiteColor_?white:black; + positionBarillet_ = arriere2; + break; + case arriere4: + stockBarillet[4] = colorSeen(frontBallRVB_)==whiteColor_?white:black; + positionBarillet_ = arriere3; + break; + default: + break; + } + log_ ("Es::Barillet", Log::debug) << "gobage balle par l'arriere."; +} + /// Update system for one sensor void Es::updateAnalysisSensor (int value, int index, int threshold) @@ -479,6 +678,15 @@ Es::updateAnalysisSensor (int value, int index, int threshold) seenColors_[index] = lastSeenColors_[index]; } +/// Say if barillet is full +bool +Es::barilletIsFull(void) +{ + for (int i = 0; i < 5; i++) + if (stockBarillet[0] == empty) + return false; + return true; +} /// What color do you see my lord ? int Es::colorSeen (int sensor_num) diff --git a/i/marvin/src/es/es.hh b/i/marvin/src/es/es.hh index 600a480..8abcca4 100644 --- a/i/marvin/src/es/es.hh +++ b/i/marvin/src/es/es.hh @@ -35,6 +35,11 @@ class Config; /// Gère la carte es et ses capteurs et actionneurs class Es : public Proto::Receiver { + public: + enum etatBarillet{empty,white,black}; + enum posBar{avant0 = 0x00, arriere2 = 0x04, avant4 = 0x08, arriere1 = + 0x0C, avant3 = 0x10, arriere0 = 0x14, avant2 = 0x18, arriere4 = 0x1C, + avant1 = 0x20, arriere3 = 0x24}; typedef std::vector Sharps; private: @@ -72,10 +77,14 @@ class Es : public Proto::Receiver /// Stat of front sensors int rvbSniffFrontStat_; int thresholdFrontSensors_, thresholdBallSensors_, thresholdHoleSensors_; - - + /// Stock du barillet + etatBarillet stockBarillet[5]; + /// Position du barillet + posBar positionBarillet_; /// System for analyse a ball int seenColors_[5], lastSeenColors_[5], comptSeenColors_[5]; + /// Si la turbine avant est allumé + bool frontTurbineIsFull_; public: // Some fucking defines ! @@ -160,18 +169,30 @@ class Es : public Proto::Receiver void barilletSleep (void); /// Dépose une balle du barillet void deposeBalle(void); + /// Dépose une balle blanche + bool dropWhiteBall(void); + /// Dépose une balle noire + bool dropBlackBall(void); + /// Place un trou vide à l'avant + bool setEmptyHoleFront(void); /// Extrait une balle void extraitBalle(void); /// Rotation du barillet void rotationBarillet(int posFinal); /// Empty everything in the barillet void barilletEmpty (void); + /// Say if barillet is full + bool barilletIsFull(void); /// What color do you see my lord ? int colorSeen (int sensor_num); private: /// Decode a color into a string std::string decodeColor (int color); + /// Analyse une balle arrivant par devant + void newBallFront (void); + /// Analyse une balle arrivant par derrière + void newBallRear (void); /// Update system for one sensor void updateAnalysisSensor (int value, int index, int threshold); }; diff --git a/i/marvin/src/es/test_es.cc b/i/marvin/src/es/test_es.cc index e5c90e5..a0e2a7c 100644 --- a/i/marvin/src/es/test_es.cc +++ b/i/marvin/src/es/test_es.cc @@ -110,6 +110,15 @@ class TestEs : public Tester interpreter.add ("turbDepose", Interpreter::memFunc (es_, &Es::deposeBalle), "Drop a ball at the rear ()"); + interpreter.add + ("barDropWhite", Interpreter::memFunc (es_, &Es::dropWhiteBall), + "drop une balle blanche"); + interpreter.add("barDropBlack", Interpreter::memFunc (es_, + &Es::dropBlackBall), + "drop une balle noire"); + interpreter.add("barSetEmpty", Interpreter::memFunc (es_, + &Es::setEmptyHoleFront), + "met un trou vide en position avale une balle"); interpreter.add ("barExtract", Interpreter::memFunc (es_, &Es::extraitBalle), "Extract a ball ()"); diff --git a/i/marvin/src/motor/motor.cc b/i/marvin/src/motor/motor.cc index 5f3987a..8bc6037 100644 --- a/i/marvin/src/motor/motor.cc +++ b/i/marvin/src/motor/motor.cc @@ -66,6 +66,26 @@ Motor::findHole (void) asserv_.findHole (seq_); } +void Motor::lockGoodHole (void) +{ + ///XXX VERIFIER LES COORDONNEES + const int xGood = 2000; + const int yGood = -1100; + ///XXX VERIFIER LES CALCULS D'AILLEURS... + double rotation = atan2((yGood - y_), (xGood - x_)); + rotate(rotation); +} + +void Motor::lockBadHole (void) +{ + ///XXX VERIFIER LES COORDONNEES + const int xGood = 600; + const int yGood = -1100; + ///XXX VERIFIER LES CALCULS D'AILLEURS... + double rotation = atan2((yGood - y_), (xGood - x_)); + rotate(rotation); +} + /// Stop now. void Motor::stop (void) diff --git a/i/marvin/src/motor/motor.hh b/i/marvin/src/motor/motor.hh index 7cc7e78..1c6f403 100644 --- a/i/marvin/src/motor/motor.hh +++ b/i/marvin/src/motor/motor.hh @@ -52,6 +52,10 @@ class Motor : public Asserv::Receiver void rotate (double a); /// Find a hole. void findHole (void); + /// Lock good hole (approximatively...) + void lockGoodHole(void); + /// Lock ennemy hole (non pas son cul) + void lockBadHole(void); /// Stop now. void stop (void); /// get the file descriptor -- cgit v1.2.3