From 55e5056593ad021213e97c6418382ac051d85d93 Mon Sep 17 00:00:00 2001 From: dufourj Date: Wed, 24 May 2006 12:22:45 +0000 Subject: Ai : - commit de travaille. Es : - merge des fonctions du barillet. ES : - protection contre l'analyse quand les capteurs sont pas pret ; - fonction de gestion capteur RVB frontaux seulement. --- i/marvin/src/es/es.cc | 49 +++++++++++-- i/marvin/src/es/es.hh | 12 +++- i/marvin/src/es/test_es.cc | 171 ++++++++++++++++++++++++--------------------- 3 files changed, 145 insertions(+), 87 deletions(-) (limited to 'i/marvin/src/es') diff --git a/i/marvin/src/es/es.cc b/i/marvin/src/es/es.cc index 567830f..8bc90ec 100644 --- a/i/marvin/src/es/es.cc +++ b/i/marvin/src/es/es.cc @@ -76,6 +76,7 @@ Es::loadConfig (const Config & config) rvbSniffRefMask_ = config.get ("es.rvb_sniff_ref_mask"); rvbSniffMaskStat_ = config.get ("es.rvb_sniff_mask_stat"); rvbSniffStat_ = config.get ("es.rvb_sniff_stat"); + rvbSniffFrontStat_ = config.get ("es.rvb_sniff_front_stat"); rvbBallStat_ = config.get ("es.rvb_ball_stat"); othersStat_ = config.get ("es.others_stat"); lcdKeyStat_ = config.get ("es.lcd_key_stat"); @@ -100,6 +101,7 @@ Es::reset (void) setRVBSniffStat (rvbSniffMaskStat_, rvbSniffStat_); setRVBBallStat (rvbBallStat_); setOthersStat (othersStat_); + setRVBSniffFrontStat (rvbSniffFrontStat_); lcdGetKey (lcdKeyStat_); // We want to reference some sensors @@ -169,6 +171,13 @@ Es::setRVBBallStat (int freq) proto_.send ('B', "b", freq); } +/// Set frequency of front sensor in analyse mode +void +Es::setRVBSniffFrontStat (int freq) +{ + proto_.send ('C', "b", freq); +} + /// Enable all the sensors or just the 4 and 1 near the ground void Es::enableAllSensors (bool enable) @@ -251,12 +260,18 @@ Es::barilletInit (void) proto_.send ('i'); } -// Init n°2 of barillet -void Es::init2Barillet(void) +/// Init turbine to minimal speed +void Es::barilletDebutLancement (void) { proto_.send ('g'); } +/// Init front turbine to full speed +void Es::barilletLancement (void) +{ + proto_.send ('h'); +} + /// Put barillet in sleep mode void Es::barilletSleep (void) @@ -282,6 +297,11 @@ void Es::rotationBarillet(int posFinal) proto_.send ('t', "b", posFinal); } +/// Empty everything in the barillet +void Es::barilletEmpty (void) +{ + proto_.send ('n'); +} void Es::receive(char command, const Proto::Frame & frame) { @@ -294,7 +314,7 @@ void Es::receive(char command, const Proto::Frame & frame) case 'Z': if (proto_.decode (frame, "bbbb", stat1, stat2, stat3, stat4)) { - log_ ("Stats main : ", Log::debug) << stat1 << " " << stat2 << " " + log_ ("Stats main", Log::debug) << stat1 << " " << stat2 << " " << stat3 << " " << stat4; } break; @@ -302,6 +322,7 @@ void Es::receive(char command, const Proto::Frame & frame) case 'F': if (proto_.decode (frame, "b", errCode)) { + log_ ("Ack", Log::debug) << "ErrCode :" << errCode; switch (errCode) { case 1: @@ -315,7 +336,7 @@ void Es::receive(char command, const Proto::Frame & frame) case 'S': if (proto_.decode (frame, "wwwww", compt, red, blue, clear, green)) { - log_ ("Stats RVB Raw : ", Log::debug) << "[" << compt << "] = {" + log_ ("Stats RVB Raw", Log::debug) << "[" << compt << "] = {" << red << "," << blue << "," << clear << "," << green << "}"; } break; @@ -323,15 +344,22 @@ void Es::receive(char command, const Proto::Frame & frame) case 'A': if (proto_.decode (frame, "bb", compt, value)) { - log_ ("Stats RVB Sniff : ", Log::debug) << "[" << compt << "] = " + log_ ("Stats RVB Sniff", Log::debug) << "[" << compt << "] = " << decodeColor (value); } break; + case 'C': + if (proto_.decode (frame, "bb", stat1, stat2)) + { + log_ ("Stats RVB Front", Log::debug) << decodeColor (stat1) + << " - " << decodeColor (stat2); + } + break; /* RVB Balls */ case 'B': if (proto_.decode (frame, "bb", stat1, stat2)) { - log_ ("Stats RVB Ball : ", Log::debug) << "[" << decodeColor + log_ ("Stats RVB Ball", Log::debug) << "[" << decodeColor (stat1) << "] [" << decodeColor (stat2) << "]"; rvbBall_[0] = stat1; rvbBall_[1] = stat2; @@ -359,7 +387,7 @@ void Es::receive(char command, const Proto::Frame & frame) jack_ = colorModeBlue_ = true; break; } - log_ ("Colour mode : ", Log::debug) << (colorModeBlue_ ? "Blue" : "Red") + log_ ("Others", Log::debug) << "Color mode " << (colorModeBlue_ ? "Blue" : "Red") << (jack_ ? ", jack in..." : ", jack out !!!"); } break; @@ -376,6 +404,13 @@ void Es::receive(char command, const Proto::Frame & frame) sharps_[compt] = value; } break; + /* Barillet */ + case 'W': + if (proto_.decode (frame, "bb", stat1, stat2)) + { + log_ ("Barillet", Log::debug) << stat1 << " " << stat2; + } + break; } } diff --git a/i/marvin/src/es/es.hh b/i/marvin/src/es/es.hh index 46f1dc7..dd9ac1c 100644 --- a/i/marvin/src/es/es.hh +++ b/i/marvin/src/es/es.hh @@ -72,6 +72,8 @@ class Es : public Proto::Receiver /// Others module, jack & colour bool jack_, colorModeBlue_; int othersStat_; + /// Stat of front sensors + int rvbSniffFrontStat_; public: /// Constructeur @@ -108,6 +110,8 @@ class Es : public Proto::Receiver void setRVBSniffStat (int mask_captor, int freq); /// Configure statistic for the sensor of the ball void setRVBBallStat (int freq); + /// Set frequency of front sensor in analyse mode + void setRVBSniffFrontStat (int freq); /// Enable all the sensors or just the 4 and 1 near the ground void enableAllSensors (bool enable); @@ -136,8 +140,10 @@ class Es : public Proto::Receiver void setTheMeaningOfRotationOfBarillet (int answer); /// Init the barillet and put it at the right place void barilletInit (void); - /// Init n°2 of barillet - void init2Barillet(void); + /// Init turbine to minimal speed + void barilletDebutLancement (void); + /// Init front turbine to full speed + void barilletLancement (void); /// Put barillet in sleep mode void barilletSleep (void); /// Dépose une balle du barillet @@ -146,6 +152,8 @@ class Es : public Proto::Receiver void extraitBalle(void); /// Rotation du barillet void rotationBarillet(int posFinal); + /// Empty everything in the barillet + void barilletEmpty (void); private: /// Decode a color into a string diff --git a/i/marvin/src/es/test_es.cc b/i/marvin/src/es/test_es.cc index aa9ce6d..4213ebb 100644 --- a/i/marvin/src/es/test_es.cc +++ b/i/marvin/src/es/test_es.cc @@ -31,87 +31,102 @@ class TestEs : public Tester { - private: - Es es_; - /// Called after each command called. - void postcall (void) + private: + Es es_; + /// Called after each command called. + void postcall (void) + { + while (!es_.wait ()) + ; + } + /// Wait. + void wait (int ms) + { + int t, stop; + t = Timer::getProgramTime (); + stop = t + ms; + while (t < stop) { - while (!es_.wait ()) - ; - } - /// Wait. - void wait (int ms) - { - int t, stop; + es_.wait (stop - t); t = Timer::getProgramTime (); - stop = t + ms; - while (t < stop) - { - es_.wait (stop - t); - t = Timer::getProgramTime (); - } } - public: - // Constructor - TestEs (int argc, char ** argv) - : Tester (argc, argv), es_(config_) { } - void preRun (void) - { - Interpreter &interpreter = getInterpreter (); - // Add functions. - interpreter.add ("reset", Interpreter::memFunc (es_, &Es::reset), - "Reset ()"); - interpreter.add ("wait", Interpreter::memFunc (*this, &TestEs::wait), - "Blocking wait (time)\n" - " - time : time to wait in ms"); - interpreter.add - ("colorRef", Interpreter::memFunc (es_, &Es::setRefColor), - "Reference current color as green for sensors (mask)"); - interpreter.add - ("rvbAll", Interpreter::memFunc (es_, &Es::enableAllSensors), - "Drop a ball at the rear ()"); - interpreter.add - ("servoPos", Interpreter::memFunc (es_, &Es::setServoPos), - "Set servo position (mask, pos)"); - interpreter.add - ("sharpUp", Interpreter::memFunc (es_, &Es::setSharpUpdate), - "Set sharp update frequency (mask, freq)"); - interpreter.add - ("sharpStat", Interpreter::memFunc (es_, &Es::setSharpStat), - "Set sharp stats (mask, freq)"); - interpreter.add - ("sharpThreshold", Interpreter::memFunc (es_, &Es::setSharpThreshold), - "Set sharp threshold (num, high, low)\n" - " - num : sharp number (1-3)\n" - " - high : high threshold\n" - " - low : low threshold"); - interpreter.add - ("lcdPrint", Interpreter::memFunc (es_, &Es::lcdPrint), - "Print a message (32 char max) onto the LCD (message)"); - interpreter.add - ("lcdKey", Interpreter::memFunc (es_, &Es::lcdGetKey), - "Set stat for key if pressed (freq)"); - interpreter.add - ("turbSpeed", Interpreter::memFunc (es_, &Es::setTurbineSpeed), - "Set turbine speed (num, speed)\n" - " - num : 1 front, 2 rear\n" - " - speed : 256 - 819"); - interpreter.add - ("turbMin", Interpreter::memFunc (es_, &Es::init2Barillet), - "Turbine minimal speed ()"); - interpreter.add - ("turbDep", Interpreter::memFunc (es_, &Es::deposeBalle), - "Drop a ball at the rear ()"); - interpreter.add - ("barGoTo", Interpreter::memFunc (es_, &Es::rotationBarillet), - "Barillet go to position (pos)\n" - " - pos : in 40° of a round"); - interpreter.add - ("barSleep", Interpreter::memFunc (es_, &Es::barilletSleep), - "Barillet in sleep mode (low speed turbine)"); - interpreter.add ("_postcall", - Interpreter::memFunc (*this, &TestEs::postcall)); - } + } + public: + // Constructor + TestEs (int argc, char ** argv) + : Tester (argc, argv), es_(config_) { } + void preRun (void) + { + Interpreter &interpreter = getInterpreter (); + // Add functions. + interpreter.add ("reset", Interpreter::memFunc (es_, &Es::reset), + "Reset ()"); + interpreter.add ("wait", Interpreter::memFunc (*this, &TestEs::wait), + "Blocking wait (time)\n" + " - time : time to wait in ms"); + interpreter.add + ("colorRef", Interpreter::memFunc (es_, &Es::setRefColor), + "Reference current color as green for sensors (mask)"); + interpreter.add + ("rvbAll", Interpreter::memFunc (es_, &Es::enableAllSensors), + "Drop a ball at the rear ()"); + interpreter.add + ("servoPos", Interpreter::memFunc (es_, &Es::setServoPos), + "Set servo position (mask, pos)"); + interpreter.add + ("sharpUp", Interpreter::memFunc (es_, &Es::setSharpUpdate), + "Set sharp update frequency (mask, freq)"); + interpreter.add + ("sharpStat", Interpreter::memFunc (es_, &Es::setSharpStat), + "Set sharp stats (mask, freq)"); + interpreter.add + ("sharpThreshold", Interpreter::memFunc (es_, &Es::setSharpThreshold), + "Set sharp threshold (num, high, low)\n" + " - num : sharp number (1-3)\n" + " - high : high threshold\n" + " - low : low threshold"); + interpreter.add + ("lcdPrint", Interpreter::memFunc (es_, &Es::lcdPrint), + "Print a message (32 char max) onto the LCD (message)"); + interpreter.add + ("lcdKey", Interpreter::memFunc (es_, &Es::lcdGetKey), + "Set stat for key if pressed (freq)"); + interpreter.add + ("turbSpeed", Interpreter::memFunc (es_, &Es::setTurbineSpeed), + "Set turbine speed (num, speed)\n" + " - num : 1 front, 2 rear\n" + " - speed : 256 - 819"); + interpreter.add + ("turbMinSpeed", Interpreter::memFunc (es_, + &Es::barilletDebutLancement), + "Turbine minimal speed ()"); + interpreter.add + ("turbFrontFull", Interpreter::memFunc (es_, + &Es::barilletLancement), + "Turbine front full speed ()"); + interpreter.add + ("barSleep", Interpreter::memFunc (es_, &Es::barilletSleep), + "Barillet in sleep mode (low speed turbine)"); + interpreter.add + ("turbDepose", Interpreter::memFunc (es_, &Es::deposeBalle), + "Drop a ball at the rear ()"); + interpreter.add + ("barExtract", Interpreter::memFunc (es_, &Es::extraitBalle), + "Extract a ball ()"); + interpreter.add + ("barGoTo", Interpreter::memFunc (es_, &Es::rotationBarillet), + "Barillet go to position (pos)\n" + " - pos : in 40° of a round"); + interpreter.add + ("barPurge", Interpreter::memFunc (es_, &Es::barilletEmpty), + "Extract a ball ()"); + interpreter.add ("_postcall", + Interpreter::memFunc (*this, &TestEs::postcall)); + } + void postRun (void) + { + es_.reset (); + } }; int -- cgit v1.2.3