// 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), jack_ (false), colorModeBlue_ (false) { // Récupération des valeurs de configuration dans le fichier loadConfig (config); proto_.open (tty_); rvbBall_[0] = rvbBall_[1] = -1; sharps_.resize (3); } 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"); 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"); } void Es::reset (void) { // On reset l'AVR proto_.send ('z'); // First, enable all sensors setRVBSensorsConfig (rvbSensorFalseIC_, rvbSensorMaxOv_); enableAllSensors (true); // Send configurations setMainStat (mainStat_); setAckStat (ackFreq_); setRVBSniffConfig (rvbSniffRefRatio_, rvbSniffGreenLimit_, rvbSniffClearLimit_); setRVBSensorsStat (rvbSensorMaskStat_, rvbSensorStat_); setRVBSniffStat (rvbSniffMaskStat_, rvbSniffStat_); setRVBBallStat (rvbBallStat_); setOthersStat (othersStat_); setRVBSniffFrontStat (rvbSniffFrontStat_); lcdGetKey (lcdKeyStat_); // We want to reference some sensors // XXX We should wait ! setRefColor (rvbSniffRefMask_); // Disable useless sensors enableAllSensors (false); } /// 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_); } // 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::setRefColor(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 32 char) void Es::lcdPrint (const std::string &message) { // XXX Yerk ! std::memset (lcd_mess_char_, '\0', 32); int size = message.size (); std::memcpy (lcd_mess_char_, message.data (), size > 32 ? 32 : size); proto_.send ('l', lcd_mess_char_, 32); } /// 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'); } /// 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) { proto_.send ('s'); } // Dépose une balle du barillet void Es::deposeBalle(void) { proto_.send ('d'); } /// Extrait une balle void Es::extraitBalle(void) { proto_.send ('e'); } /// Rotation du barillet 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) { 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; } 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); } break; /* RVB Balls */ case 'B': if (proto_.decode (frame, "bb", stat1, stat2)) { log_ ("Stats RVB Ball", Log::debug) << "[" << decodeColor (stat1) << "] [" << decodeColor (stat2) << "]"; rvbBall_[0] = stat1; rvbBall_[1] = stat2; } break; /* Others */ case 'O': if (proto_.decode (frame, "b", value)) { // XXX Use a decallage please switch (value) { case 0: jack_ = colorModeBlue_ = false; break; case 1: jack_ = true; colorModeBlue_ = false; break; case 2: jack_ = false; colorModeBlue_ = true; break; case 3: jack_ = colorModeBlue_ = true; break; } log_ ("Others", Log::debug) << "Color mode " << (colorModeBlue_ ? "Blue" : "Red") << (jack_ ? ", 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"; } }