From 8f486613be58ced269db1d437e560c16558604e8 Mon Sep 17 00:00:00 2001 From: becquet Date: Thu, 10 May 2007 18:49:20 +0000 Subject: Création de chuck, le programme du robot 2007. --- i/chuck/src/es/Makefile.defs | 5 + i/chuck/src/es/es.cc | 752 +++++++++++++++++++++++++++++++++++++++++++ i/chuck/src/es/es.hh | 207 ++++++++++++ i/chuck/src/es/test_es.cc | 156 +++++++++ 4 files changed, 1120 insertions(+) create mode 100644 i/chuck/src/es/Makefile.defs create mode 100644 i/chuck/src/es/es.cc create mode 100644 i/chuck/src/es/es.hh create mode 100644 i/chuck/src/es/test_es.cc (limited to 'i/chuck/src/es') diff --git a/i/chuck/src/es/Makefile.defs b/i/chuck/src/es/Makefile.defs new file mode 100644 index 0000000..66a4e6f --- /dev/null +++ b/i/chuck/src/es/Makefile.defs @@ -0,0 +1,5 @@ +PROGRAMS += test_es + +es_OBJECTS = es.o + +test_es_OBJECTS = test_es.o $(es_OBJECTS) $(tester_OBJECTS) $(proto_OBJECTS) diff --git a/i/chuck/src/es/es.cc b/i/chuck/src/es/es.cc new file mode 100644 index 0000000..66a8fc9 --- /dev/null +++ b/i/chuck/src/es/es.cc @@ -0,0 +1,752 @@ +// 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_ +(-2), 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_); + + 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) +{ + std::memset (lcd_mess_char_, ' ', 16); + int size = message.size (); + std::memcpy (lcd_mess_char_, message.data (), size > 16 ? 16 : size); + proto_.sendStr ('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; + wait (400); + barilletLancement(); +} + +/// 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; + case 4: + // A ball has been by the sensor at the front of the barillet + enableAllSensors (true); + setRVBBallStat (5); + 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_); + log_ ("Ball") << "Devant : " << (colorSeen (frontBallRVB_) == + whiteColor_ ? "white" : "black") << " Arr : " << (colorSeen + (rearBallRVB_) == whiteColor_ ? "white" : "black"); + } + 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; + } +} + +void Es::setRVBHoleStat (int freq) +{ + proto_.send ('D', "b", freq); +} + +/// 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."; + enableAllSensors (false); + setRVBBallStat (0); +} + +/// 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:white; + positionBarillet_ = arriere4; + log_ ("Es::Barillet", Log::debug) << "trou 0"; + break; + case arriere1: + stockBarillet[1] = ((colorSeen(frontBallRVB_))==whiteColor_)?white:white; + positionBarillet_ = arriere0; + log_ ("Es::Barillet", Log::debug) << "trou 1"; + break; + case arriere2: + stockBarillet[2] = ((colorSeen(frontBallRVB_))==whiteColor_)?white:white; + positionBarillet_ = arriere1; + log_ ("Es::Barillet", Log::debug) << "trou 2"; + break; + case arriere3: + stockBarillet[3] = ((colorSeen(frontBallRVB_))==whiteColor_)?white:white; + positionBarillet_ = arriere2; + log_ ("Es::Barillet", Log::debug) << "trou 3"; + break; + case arriere4: + stockBarillet[4] = ((colorSeen(frontBallRVB_))==whiteColor_)?white:white; + positionBarillet_ = arriere3; + log_ ("Es::Barillet", Log::debug) << "trou 4"; + break; + default: + break; + } + log_ ("Es::Barillet", Log::debug) << "gobage balle par l'arriere."; + enableAllSensors (false); + setRVBBallStat (0); +} + +/// 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]; +} + +/// Totems activator +void +Es::totemActivator (bool out) +{ + // Right + setServoPos (2, out ? 0 : 200); + // Left + setServoPos (4, out ? 200 : 0); +} diff --git a/i/chuck/src/es/es.hh b/i/chuck/src/es/es.hh new file mode 100644 index 0000000..0a53ff3 --- /dev/null +++ b/i/chuck/src/es/es.hh @@ -0,0 +1,207 @@ +#ifndef es_hh +#define es_hh +// es.hh +// 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 "proto/proto.hh" + +#include +#include + +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: + // Objet Proto de communication vers la carte es + Proto proto_; + // le tty est ... + std::string tty_; + /// Système de log + Log log_; + /// For LCD communication + char lcd_mess_char_[16]; + /// Key pressed by the LCD Keyboard + int lcdKeyPressed_, lcdKeyStat_; + /// Frontal choc ! + bool front_sensor_; + /// Ack frequency + int ackFreq_; + /// Main stat frequency + int mainStat_; + /// Config of RVB sensors + int rvbSensorFalseIC_, rvbSensorMaxOv_; + /// Config of RVB sniff + int rvbSniffRefRatio_, rvbSniffGreenLimit_, rvbSniffClearLimit_; + /// RVB Sensors raw stats + int rvbSensorMaskStat_, rvbSensorStat_; + /// RVB Sniff stats + int rvbSniffMaskStat_, rvbSniffStat_; + /// RVB Sensors raw stats + int rvbBallStat_; + /// Sharps + Sharps sharps_; + /// Others module, jack & colour + bool jackIn_, colorModeBlue_; + int othersStat_; + /// 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 ! + const int unknownColor_, redColor_, blueColor_, greenColor_, whiteColor_, + blackColor_; + const int leftFrontRVB_, rightFrontRVB_, holeRVB_, frontBallRVB_, + rearBallRVB_; + /// Constructeur + Es (const Config & config); + /// On attend ... + bool wait (int timeout = -1); + /// Récupère le File Descriptor + int getFd (void); + /// définition du receiver::receive + void receive (char command, const Proto::Frame & frame); + /// sync.. + bool sync (void); + /// Charge les paramètre du fichier de config + void loadConfig (const Config & config); + + /// Initialise les capteurs et actionneurs, reset + void reset (void); + /// Stat for the main () + void setMainStat (int freq); + /// Shut up ! + void shutUp (void); + /// Set frequency of ack + void setAckStat (int freq); + /// Get the state of the jack + bool isJackOut (void); + /// Get the color mode of the button + bool isColorModeBlue (void); + + /// Envoie de la config des sensors RVB + void setRVBSensorsConfig(int false_ic, int max_ov); + /// Envoie de la config des sniff RVB + void setRVBSniffConfig(int ref_ratio, int green_limit, int clear_limit); + /// Règle les stats des sensors RVB + void setRVBSensorsStat(int mask_captor, int freq); + /// règle la couleur actuelle comme référente + void refColor(int mask_captor, int mode = 0); + /// Règle les stats d'affichage de la couleur + 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); + // + void setRVBHoleStat (int freq); + + /// Set frequency of jack, selectoul printed out function + /// Use this function with frequency 0 to disable this stat. + void setOthersStat (int freq); + + /// Discute avec les servo... + void setServoPos (int servo_mask, int servoPos); + + /// Set update frequency of sharps + void setSharpUpdate (int sharp_mask, int freq); + /// Set statistics frequency of sharps + void setSharpStat (int sharp_mask, int freq); + /// Set configuration of threshold sharps + void setSharpThreshold (int sharp_num, int threshold_high, + int threshold_low); + + /// Print something on the LCD (max 32 char) + void lcdPrint (const std::string &message); + /// Get the current pressed keys of the LCD + void lcdGetKey (int freq); + + /// Règle la vitesse des turbines + void setTurbineSpeed (int turbNb, int speed); + /// Règle le sens de rotation du barillet + void setTheMeaningOfRotationOfBarillet (int answer); + /// Init the barillet and put it at the right place + void barilletInit (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 + 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); + /// Totems activator + void totemActivator (bool out); + /// Front sensor + bool frontSensor (void) { return front_sensor_; } + /// Clear sensor + void clearFrontSensor (void) { front_sensor_ = false; } + + 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); +}; +#endif // es_hh diff --git a/i/chuck/src/es/test_es.cc b/i/chuck/src/es/test_es.cc new file mode 100644 index 0000000..2990d73 --- /dev/null +++ b/i/chuck/src/es/test_es.cc @@ -0,0 +1,156 @@ +// test_es.cc +// marvin - programme du robot 2006. {{{ +// +// Copyright (C) 2006 Nicolas Haller +// +// Robot APB Team/Efrei 2006. +// 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 "tester/tester.hh" +#include "es/es.hh" +#include "timer/timer.hh" + +#include + +class TestEs : public Tester +{ + 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) + { + 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::refColor), + "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 + ("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 ()"); + interpreter.add + ("barInit", Interpreter::memFunc (es_, &Es::barilletInit), + "Init barillet ()"); + 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 +main (int argc, char **argv) +{ + try + { + TestEs te (argc, argv); + te.run (); + } + catch (const std::exception &e) + { + std::cerr << e.what () << std::endl; + } +} -- cgit v1.2.3