// es.cc // robert - programme du robot 2005 {{{ // // Copyright (C) 2005 Nicolas Haller // // Robot APB Team/Efrei 2005. // Web: http://assos.efrei.fr/robot/ // Email: robot AT efrei DOT fr // // 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. // // }}} #include "es/es.hh" #include "config/config.hh" #include #include /// 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), blackColor_ (4), leftFrontRVB_ (0), rightFrontRVB_ (1), holeRVB_ (2), frontBallRVB_ (3), rearBallRVB_ (4) { // Récupération des valeurs de configuration dans le fichier loadConfig (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++) { seenColors_[compt] = unknownColor_; lastSeenColors_[compt] = unknownColor_; comptSeenColors_[compt] = 0; } } bool Es::wait (int timeout /*-1*/) { return proto_.wait (timeout); } /// Récupère le File Descriptor int Es::getFd (void) { return proto_.getFd(); } bool Es::sync (void) { return proto_.sync (); } void Es::loadConfig (const Config & config) { tty_ = config.get("es.tty"); ackFreq_ = config.get ("es.ack_freq"); mainStat_ = config.get ("es.main_stat"); rvbSensorFalseIC_ = config.get ("es.rvb_sensors_false_ic"); rvbSensorMaxOv_ = config.get ("es.rvb_sensors_max_ov"); rvbSniffRefRatio_ = config.get ("es.rvb_sniff_ref_ratio"); rvbSniffGreenLimit_ = config.get ("es.rvb_sniff_green_limit"); rvbSniffClearLimit_ = config.get ("es.rvb_sniff_clear_limit"); rvbSensorMaskStat_ = config.get ("es.rvb_sensor_mask_stat"); rvbSensorStat_ = config.get ("es.rvb_sensor_stat"); 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"); thresholdFrontSensors_ = config.get ("es.threshold_front_sensors"); thresholdBallSensors_ = config.get ("es.threshold_ball_sensors"); thresholdHoleSensors_ = config.get ("es.threshold_hole_sensors"); } void Es::reset (void) { // On reset l'AVR proto_.send ('z'); // First, enable all sensors setRVBSensorsConfig (rvbSensorFalseIC_, rvbSensorMaxOv_); // Send configurations setMainStat (mainStat_); setAckStat (ackFreq_); setRVBSniffConfig (rvbSniffRefRatio_, rvbSniffGreenLimit_, rvbSniffClearLimit_); setRVBSensorsStat (rvbSensorMaskStat_, rvbSensorStat_); setRVBSniffStat (rvbSniffMaskStat_, rvbSniffStat_); setRVBBallStat (rvbBallStat_); setOthersStat (othersStat_); setRVBSniffFrontStat (rvbSniffFrontStat_); lcdGetKey (lcdKeyStat_); lcdPrint ("I'm ready Tb!"); enableAllSensors (true); log_ ("Es", Log::debug) << "Reset Es done."; } /// Stat for the main () void Es::setMainStat (int freq) { proto_.send ('Z', "b", freq); } /// Shut up ! void Es::shutUp (void) { proto_.send ('f'); } /// Set frequency of ack void Es::setAckStat (int freq) { ackFreq_ = freq; proto_.send ('F', "b", ackFreq_); } /// Get the state of the jack bool Es::isJackOut (void) { return !jackIn_; } /// Get the color mode of the button bool Es::isColorModeBlue (void) { return colorModeBlue_; } // Envoie de la config des sensors RVB void Es::setRVBSensorsConfig(int false_ic, int max_ov) { proto_.send ('p',"bb", false_ic, max_ov); } // Envoie de la config des sniff RVB void Es::setRVBSniffConfig (int ref_ratio, int green_limit, int clear_limit) { proto_.send ('x', "bww", ref_ratio, green_limit, clear_limit); } // Règle les stats des sensors RVB void Es::setRVBSensorsStat(int mask_captor, int freq) { proto_.send ('S', "wb", mask_captor, freq); } // règle la couleur actuelle comme référente void Es::refColor(int mask_captor, int mode) { proto_.send ('r', "wb", mask_captor, mode); } /// Règle les stats d'affichage de la couleur void Es::setRVBSniffStat (int mask_captor, int freq) { proto_.send ('A', "wb", mask_captor, freq); } /// Configure statistic for the sensor of the ball void 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) { if (enable) proto_.send ('u', "b", 1); else proto_.send ('u', "b", 0); } /// Set frequency of jack, selectoul printed out function void Es::setOthersStat (int freq) { proto_.send ('O', "b", freq); } // Discute avec les servo... void Es::setServoPos (int servo_mask, int servoPos) { proto_.send ('m', "bb", servo_mask, servoPos); } /// Set update frequency of sharps void Es::setSharpUpdate (int sharp_mask, int freq) { proto_.send ('h', "bb", sharp_mask, freq); } /// Set statistics frequency of sharps void Es::setSharpStat (int sharp_mask, int freq) { proto_.send ('H', "bb", sharp_mask, freq); } /// Set configuration of threshold sharps void Es::setSharpThreshold (int sharp_num, int threshold_high, int threshold_low) { proto_.send ('o', "bww", sharp_num, threshold_high, threshold_low); } /// Print something on the LCD (max 16 char) void Es::lcdPrint (const std::string &message) { // XXX Yerk ! std::memset (lcd_mess_char_, '\0', 17); int size = message.size (); std::memcpy (lcd_mess_char_, message.data (), size > 16 ? 16 : size); proto_.send ('l', lcd_mess_char_, 16); } /// Get the current pressed keys of the LCD void Es::lcdGetKey (int freq) { proto_.send ('L', "b", freq); } // Règle la vitesse des turbines void Es::setTurbineSpeed(int turbNb, int speed) { proto_.send ('v', "bw", turbNb, speed); } // Règle le sens de rotation du barillet void Es::setTheMeaningOfRotationOfBarillet(int answer) { proto_.send ('w', "b", answer); } /// Init the barillet and put it at the right place 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 void Es::barilletSleep (void) { proto_.send ('s'); frontTurbineIsFull_ = false; } // Dépose une balle du barillet void Es::deposeBalle(void) { proto_.send ('d'); barilletDebutLancement(); log_ ("Es::Barillet", Log::debug) << "DROP!! "; frontTurbineIsFull_ = false; } /// Dépose une balle blanche bool Es::dropWhiteBall(void) { for(int i = 0; i < 5; i++) if(stockBarillet[i] == white) { log_ ("Es::Barillet", Log::debug) << "Balle blanche detected en trou: " << i; switch (i) { case 0: rotationBarillet(arriere0); positionBarillet_ = arriere0; stockBarillet[0] = empty; break; case 1: rotationBarillet(arriere1); positionBarillet_ = arriere1; stockBarillet[1] = empty; break; case 2: rotationBarillet(arriere2); positionBarillet_ = arriere2; stockBarillet[2] = empty; break; case 3: rotationBarillet(arriere3); positionBarillet_ = arriere3; stockBarillet[3] = empty; break; case 4: rotationBarillet(arriere4); positionBarillet_ = arriere4; stockBarillet[4] = empty; break; } //deposeBalle(); return true; } return false; } /// Dépose une balle noire bool Es::dropBlackBall(void) { for(int i = 0; i < 5; i++) if(stockBarillet[i] == black) { log_ ("Es::Barillet", Log::debug) << "Balle noire detected en trou: " << i; switch (i) { case 0: rotationBarillet(arriere0); positionBarillet_ = arriere0; stockBarillet[0] = empty; break; case 1: rotationBarillet(arriere1); positionBarillet_ = arriere1; stockBarillet[1] = empty; break; case 2: rotationBarillet(arriere2); positionBarillet_ = arriere2; stockBarillet[2] = empty; break; case 3: rotationBarillet(arriere3); positionBarillet_ = arriere3; stockBarillet[3] = empty; break; case 4: rotationBarillet(arriere4); positionBarillet_ = arriere4; stockBarillet[4] = empty; break; } //deposeBalle(); 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) { log_ ("Es::Barillet", Log::debug) << "vide detected en trou: " << i; 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) { 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) { int errCode, red, blue, clear, green, compt, value; int stat1, stat2, stat3, stat4; switch (command) { /* Main stat */ case 'Z': if (proto_.decode (frame, "bbbb", stat1, stat2, stat3, stat4)) { log_ ("Stats main", Log::debug) << stat1 << " " << stat2 << " " << stat3 << " " << stat4; } break; /* Ack */ case 'F': if (proto_.decode (frame, "b", errCode)) { log_ ("Ack", Log::debug) << "ErrCode :" << errCode; switch (errCode) { 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 (); } break; /* RVB Sensors raw stat */ case 'S': if (proto_.decode (frame, "wwwww", compt, red, blue, clear, green)) { log_ ("Stats RVB Raw", Log::debug) << "[" << compt << "] = {" << red << "," << blue << "," << clear << "," << green << "}"; } break; /* RVB Sniff stats */ case 'A': if (proto_.decode (frame, "bb", compt, value)) { 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); updateAnalysisSensor (stat2, leftFrontRVB_, thresholdFrontSensors_); updateAnalysisSensor (stat1, rightFrontRVB_, thresholdFrontSensors_); } break; /* RVB Hole */ case 'D': if (proto_.decode (frame, "b", stat1)) { log_ ("Stats RVB Hole", Log::debug) << decodeColor (stat1); updateAnalysisSensor (stat1, holeRVB_, thresholdHoleSensors_); } break; /* RVB Balls */ case 'B': if (proto_.decode (frame, "bb", stat1 /* Rear */, stat2 /* Front */)) { //log_ ("Stats RVB Ball", Log::debug) << "[" << decodeColor // (stat1) << "] [" << decodeColor (stat2) << "]"; updateAnalysisSensor (stat1, rearBallRVB_, thresholdBallSensors_); updateAnalysisSensor (stat2, frontBallRVB_, thresholdBallSensors_); } break; /* Others */ case 'O': if (proto_.decode (frame, "b", value)) { // XXX Use a decallage please if (value & 0x01) jackIn_ = true; else jackIn_ = false; if (value & 0x02) colorModeBlue_ = true; else colorModeBlue_ = false; // log_ ("Others", Log::debug) << "Color mode " << (colorModeBlue_ ? "Blue" : "Red") // << (jackIn_ ? ", jack in..." : ", jack out !!!"); } break; /* LCD */ case 'L': if (proto_.decode (frame, "b", value)) lcdKeyPressed_ = value; break; /* Sharps */ case 'H': if (proto_.decode (frame, "ww", compt, value)) { std::cout << "[" << compt << "]" << " = " << value << std::endl; sharps_[compt] = value; } break; /* Barillet */ case 'W': if (proto_.decode (frame, "bb", stat1, stat2)) { log_ ("Barillet", Log::debug) << stat1 << " " << stat2; } break; } } /// Decode a color into a string std::string Es::decodeColor (int color) { switch (color) { case 0: return "green"; break; case 1: return "blue"; break; case 2: return "red"; break; case 3: return "other"; break; case 4: return "black"; break; case 5: return "white"; break; default: return "unknow"; } } /// 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:white; positionBarillet_ = avant4; log_ ("Es::Barillet", Log::debug) << "trou 0:" << ((stockBarillet[0] == white)?"white":"pas white"); break; case avant1: stockBarillet[1] = colorSeen(frontBallRVB_)==whiteColor_?white:white; positionBarillet_ = avant0; log_ ("Es::Barillet", Log::debug) << "trou 1:" << ((stockBarillet[1] == white)?"white":"pas white"); break; case avant2: stockBarillet[2] = colorSeen(frontBallRVB_)==whiteColor_?white:white; positionBarillet_ = avant1; log_ ("Es::Barillet", Log::debug) << "trou 2:" << ((stockBarillet[2] == white)?"white":"pas white"); break; case avant3: stockBarillet[3] = colorSeen(frontBallRVB_)==whiteColor_?white:white; positionBarillet_ = avant2; log_ ("Es::Barillet", Log::debug) << "trou 3:" << ((stockBarillet[3] == white)?"white":"pas white"); break; case avant4: stockBarillet[4] = colorSeen(frontBallRVB_)==whiteColor_?white:white; positionBarillet_ = avant3; log_ ("Es::Barillet", Log::debug) << "trou 4:" << ((stockBarillet[4] == white)?"white":"pas white"); break; default: log_ ("Es::Barillet", Log::debug) << "trou 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; log_ ("Es::Barillet", Log::debug) << "trou 0"; break; case arriere1: stockBarillet[1] = colorSeen(frontBallRVB_)==whiteColor_?white:black; positionBarillet_ = arriere0; log_ ("Es::Barillet", Log::debug) << "trou 1"; break; case arriere2: stockBarillet[2] = colorSeen(frontBallRVB_)==whiteColor_?white:black; positionBarillet_ = arriere1; log_ ("Es::Barillet", Log::debug) << "trou 2"; break; case arriere3: stockBarillet[3] = colorSeen(frontBallRVB_)==whiteColor_?white:black; positionBarillet_ = arriere2; log_ ("Es::Barillet", Log::debug) << "trou 3"; break; case arriere4: stockBarillet[4] = colorSeen(frontBallRVB_)==whiteColor_?white:black; positionBarillet_ = arriere3; log_ ("Es::Barillet", Log::debug) << "trou 4"; 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) { if (value == lastSeenColors_[index]) comptSeenColors_[index]++; else { comptSeenColors_[index] = 0; lastSeenColors_[index] = value; } if (comptSeenColors_[index] > threshold) seenColors_[index] = lastSeenColors_[index]; } /// Say if barillet is full bool Es::barilletIsFull(void) { for (int i = 0; i < 5; i++) if (stockBarillet[i] == empty) return false; return true; } /// What color do you see my lord ? int Es::colorSeen (int sensor_num) { return seenColors_[sensor_num]; }