summaryrefslogtreecommitdiff
path: root/i/marvin/src/es/es.cc
diff options
context:
space:
mode:
Diffstat (limited to 'i/marvin/src/es/es.cc')
-rw-r--r--i/marvin/src/es/es.cc264
1 files changed, 187 insertions, 77 deletions
diff --git a/i/marvin/src/es/es.cc b/i/marvin/src/es/es.cc
index b397cef..567830f 100644
--- a/i/marvin/src/es/es.cc
+++ b/i/marvin/src/es/es.cc
@@ -31,55 +31,104 @@
/// Constructeur
Es::Es (const Config & config)
- : proto_ (*this), log_ ("Es"), lcd_key_pressed_ (-1),
- front_sensor_ (false)
+ : proto_ (*this), log_ ("Es"), lcdKeyPressed_ (-1),
+ front_sensor_ (false), jack_ (false), colorModeBlue_ (false)
{
// Récupération des valeurs de configuration dans le fichier
loadConfig (config);
proto_.open (tty_);
- sensorsRvb_.resize (9);
- for (int compt = 0; compt < 9; compt++)
- sensorsRvb_[0].resize (4);
- sniffRvb_.resize (9);
+ rvbBall_[0] = rvbBall_[1] = -1;
sharps_.resize (3);
}
+bool
+Es::wait (int timeout /*-1*/)
+{
+ return proto_.wait (timeout);
+}
+
+/// Récupère le File Descriptor
+int
+Es::getFd (void)
+{
+ return proto_.getFd();
+}
+
+bool Es::sync (void)
+{
+ return proto_.sync ();
+}
+
+void
+Es::loadConfig (const Config & config)
+{
+ tty_ = config.get<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");
+ rvbSniffRefMask_ = config.get<int> ("es.rvb_sniff_ref_mask");
+ rvbSniffMaskStat_ = config.get<int> ("es.rvb_sniff_mask_stat");
+ rvbSniffStat_ = config.get<int> ("es.rvb_sniff_stat");
+ rvbBallStat_ = config.get<int> ("es.rvb_ball_stat");
+ othersStat_ = config.get<int> ("es.others_stat");
+ lcdKeyStat_ = config.get<int> ("es.lcd_key_stat");
+}
+
void
Es::reset (void)
{
// On reset l'AVR
proto_.send ('z');
- // Send configuration TODO
- // We want to reference some sensors
+
// First, enable all sensors
+ setRVBSensorsConfig (rvbSensorFalseIC_, rvbSensorMaxOv_);
enableAllSensors (true);
- /// XXX Is it to often ?
-// setAckStat (1);
+
+ // Send configurations
+ setMainStat (mainStat_);
+ setAckStat (ackFreq_);
+ setRVBSniffConfig (rvbSniffRefRatio_, rvbSniffGreenLimit_,
+ rvbSniffClearLimit_);
+ setRVBSensorsStat (rvbSensorMaskStat_, rvbSensorStat_);
+ setRVBSniffStat (rvbSniffMaskStat_, rvbSniffStat_);
+ setRVBBallStat (rvbBallStat_);
+ setOthersStat (othersStat_);
+ lcdGetKey (lcdKeyStat_);
+
+ // We want to reference some sensors
// XXX We should wait !
- // Reference sensors 1-7
- setRefColor (127, 0);
+ setRefColor (rvbSniffRefMask_);
// Disable useless sensors
enableAllSensors (false);
}
-bool
-Es::wait (int timeout /*-1*/)
+/// Stat for the main ()
+void
+Es::setMainStat (int freq)
{
- return proto_.wait (timeout);
+ proto_.send ('Z', "b", freq);
}
-/// Récupère le File Descriptor
-int
-Es::getFd (void)
+/// Shut up !
+void
+Es::shutUp (void)
{
- return proto_.getFd();
+ proto_.send ('f');
}
+/// Set frequency of ack
void
-Es::loadConfig (const Config & config)
+Es::setAckStat (int freq)
{
- tty_ = config.get<std::string>("es.tty");
+ ackFreq_ = freq;
+ proto_.send ('F', "b", ackFreq_);
}
// Envoie de la config des sensors RVB
@@ -101,7 +150,7 @@ void Es::setRVBSensorsStat(int mask_captor, int freq)
}
// règle la couleur actuelle comme référente
-void Es::setRefColor(int mask_captor, int mode = 0)
+void Es::setRefColor(int mask_captor, int mode)
{
proto_.send ('r', "wb", mask_captor, mode);
}
@@ -115,9 +164,26 @@ Es::setRVBSniffStat (int mask_captor, int freq)
/// Configure statistic for the sensor of the ball
void
-Es::setRVBBallStat (int sensor_num, int freq)
+Es::setRVBBallStat (int freq)
+{
+ proto_.send ('B', "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 ('B', "bb", sensor_num, freq);
+ proto_.send ('O', "b", freq);
}
// Discute avec les servo...
@@ -156,14 +222,14 @@ Es::lcdPrint (const std::string &message)
std::memset (lcd_mess_char_, '\0', 32);
int size = message.size ();
std::memcpy (lcd_mess_char_, message.data (), size > 32 ? 32 : size);
- proto_.send ('L', lcd_mess_char_, 32);
+ proto_.send ('l', lcd_mess_char_, 32);
}
/// Get the current pressed keys of the LCD
void
Es::lcdGetKey (int freq)
{
- proto_.send ('l', "b", freq);
+ proto_.send ('L', "b", freq);
}
// Règle la vitesse des turbines
@@ -178,12 +244,26 @@ void Es::setTheMeaningOfRotationOfBarillet(int answer)
proto_.send ('w', "b", answer);
}
+/// Init the barillet and put it at the right place
+void
+Es::barilletInit (void)
+{
+ proto_.send ('i');
+}
+
// Init n°2 of barillet
void Es::init2Barillet(void)
{
proto_.send ('g');
}
+/// Put barillet in sleep mode
+void
+Es::barilletSleep (void)
+{
+ proto_.send ('s');
+}
+
// Dépose une balle du barillet
void Es::deposeBalle(void)
{
@@ -202,50 +282,23 @@ void Es::rotationBarillet(int posFinal)
proto_.send ('t', "b", posFinal);
}
-/// Put barillet in sleep mode
-void
-Es::barilletSleep (void)
-{
- proto_.send ('s');
-}
-
-/// 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);
-}
-
-/// Shut up !
-void
-Es::shutUp (void)
-{
- proto_.send ('f', "b", 0);
-}
-
-/// Set frequency of ack
-void
-Es::setAckStat (int freq)
-{
- proto_.send ('F', "b", freq);
-}
-
-/// Stat for the main ()
-void
-Es::setMainStat (int freq)
-{
- proto_.send ('Z', "b", freq);
-}
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))
{
@@ -258,30 +311,64 @@ void Es::receive(char command, const Proto::Frame & frame)
shutUp ();
}
break;
- case 'Z':
- // TODO
- break;
- case 'S':
+ /* RVB Sensors raw stat */
+ case 'S':
if (proto_.decode (frame, "wwwww", compt, red, blue, clear, green))
{
- sensorsRvb_[compt][0] = red;
- sensorsRvb_[compt][1] = blue;
- sensorsRvb_[compt][2] = clear;
- sensorsRvb_[compt][3] = 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))
{
- sniffRvb_[compt] = value;
+ log_ ("Stats RVB Sniff : ", Log::debug) << "[" << compt << "] = "
+ << decodeColor (value);
}
break;
+ /* RVB Balls */
case 'B':
- if (proto_.decode (frame, "bb", compt, value))
+ if (proto_.decode (frame, "bb", stat1, stat2))
{
- sniffRvb_[compt] = value;
+ log_ ("Stats RVB Ball : ", Log::debug) << "[" << decodeColor
+ (stat1) << "] [" << decodeColor (stat2) << "]";
+ rvbBall_[0] = stat1;
+ rvbBall_[1] = stat2;
}
break;
+ /* Others */
+ case 'O':
+ if (proto_.decode (frame, "b", value))
+ {
+ // XXX Use a decallage please
+ switch (value)
+ {
+ case 0:
+ jack_ = colorModeBlue_ = false;
+ break;
+ case 1:
+ jack_ = true;
+ colorModeBlue_ = false;
+ break;
+ case 2:
+ jack_ = false;
+ colorModeBlue_ = true;
+ break;
+ case 3:
+ jack_ = colorModeBlue_ = true;
+ break;
+ }
+ log_ ("Colour mode : ", Log::debug) << (colorModeBlue_ ? "Blue" : "Red")
+ << (jack_ ? ", jack in..." : ", jack out !!!");
+ }
+ break;
+ /* LCD */
+ case 'L':
+ if (proto_.decode (frame, "b", value))
+ lcdKeyPressed_ = value;
+ break;
+ /* Sharps */
case 'H':
if (proto_.decode (frame, "ww", compt, value))
{
@@ -292,8 +379,31 @@ void Es::receive(char command, const Proto::Frame & frame)
}
}
-bool Es::sync (void)
+/// Decode a color into a string
+std::string
+Es::decodeColor (int color)
{
- return proto_.sync ();
+ 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";
+ }
}
-