summaryrefslogtreecommitdiff
path: root/i/chuck/src/es
diff options
context:
space:
mode:
authorbecquet2007-05-10 18:49:20 +0000
committerbecquet2007-05-10 18:49:20 +0000
commit8f486613be58ced269db1d437e560c16558604e8 (patch)
tree41e94b2122a118cb06abf6fc2a0038cd1dfbec4a /i/chuck/src/es
parent4daa2c76c2a028e4b2c8ab379e7d1e0f535a0a31 (diff)
Création de chuck, le programme du robot 2007.
Diffstat (limited to 'i/chuck/src/es')
-rw-r--r--i/chuck/src/es/Makefile.defs5
-rw-r--r--i/chuck/src/es/es.cc752
-rw-r--r--i/chuck/src/es/es.hh207
-rw-r--r--i/chuck/src/es/test_es.cc156
4 files changed, 1120 insertions, 0 deletions
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 <cstring>
+#include <iostream>
+
+/// 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<std::string>("es.tty");
+ ackFreq_ = config.get<int> ("es.ack_freq");
+ mainStat_ = config.get<int> ("es.main_stat");
+ rvbSensorFalseIC_ = config.get<int> ("es.rvb_sensors_false_ic");
+ rvbSensorMaxOv_ = config.get<int> ("es.rvb_sensors_max_ov");
+ rvbSniffRefRatio_ = config.get<int> ("es.rvb_sniff_ref_ratio");
+ rvbSniffGreenLimit_ = config.get<int> ("es.rvb_sniff_green_limit");
+ rvbSniffClearLimit_ = config.get<int> ("es.rvb_sniff_clear_limit");
+ rvbSensorMaskStat_ = config.get<int> ("es.rvb_sensor_mask_stat");
+ rvbSensorStat_ = config.get<int> ("es.rvb_sensor_stat");
+ rvbSniffMaskStat_ = config.get<int> ("es.rvb_sniff_mask_stat");
+ rvbSniffStat_ = config.get<int> ("es.rvb_sniff_stat");
+ rvbSniffFrontStat_ = config.get<int> ("es.rvb_sniff_front_stat");
+ rvbBallStat_ = config.get<int> ("es.rvb_ball_stat");
+ othersStat_ = config.get<int> ("es.others_stat");
+ lcdKeyStat_ = config.get<int> ("es.lcd_key_stat");
+ thresholdFrontSensors_ = config.get<int> ("es.threshold_front_sensors");
+ thresholdBallSensors_ = config.get<int> ("es.threshold_ball_sensors");
+ thresholdHoleSensors_ = config.get<int> ("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 <string>
+#include <vector>
+
+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<int> 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 <iostream>
+
+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;
+ }
+}