summaryrefslogtreecommitdiff
path: root/i/chuck/src/es/es.cc
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/es.cc
parent4daa2c76c2a028e4b2c8ab379e7d1e0f535a0a31 (diff)
Création de chuck, le programme du robot 2007.
Diffstat (limited to 'i/chuck/src/es/es.cc')
-rw-r--r--i/chuck/src/es/es.cc752
1 files changed, 752 insertions, 0 deletions
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);
+}