summaryrefslogtreecommitdiff
path: root/i/chuck/src/es/es.cc
diff options
context:
space:
mode:
Diffstat (limited to 'i/chuck/src/es/es.cc')
-rw-r--r--i/chuck/src/es/es.cc519
1 files changed, 7 insertions, 512 deletions
diff --git a/i/chuck/src/es/es.cc b/i/chuck/src/es/es.cc
index 66a8fc9..12a6f3d 100644
--- a/i/chuck/src/es/es.cc
+++ b/i/chuck/src/es/es.cc
@@ -1,11 +1,11 @@
// es.cc
-// robert - programme du robot 2005 {{{
+// chuck - programme du robot 2007 {{{
//
-// Copyright (C) 2005 Nicolas Haller
+// Copyright (C) 2007 Romain Becquet
//
-// Robot APB Team/Efrei 2005.
-// Web: http://assos.efrei.fr/robot/
-// Email: robot AT efrei DOT fr
+// Robot APB Team/Efrei 2007.
+// Web: http://assos.efrei.fr/si2e/
+// Email: si2e 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
@@ -32,27 +32,14 @@
/// 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)
+ jackIn_ (false),colorModeBlue_(false)
+
{
// 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
@@ -79,21 +66,9 @@ 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");
}
@@ -103,19 +78,10 @@ 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);
@@ -158,51 +124,6 @@ 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)
@@ -265,205 +186,6 @@ 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;
@@ -486,70 +208,10 @@ void Es::receive(char command, const Proto::Frame & frame)
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))
@@ -580,173 +242,6 @@ void Es::receive(char command, const Proto::Frame & frame)
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);
-}