summaryrefslogtreecommitdiff
path: root/i/marvin/src/es
diff options
context:
space:
mode:
Diffstat (limited to 'i/marvin/src/es')
-rw-r--r--i/marvin/src/es/es.cc73
-rw-r--r--i/marvin/src/es/es.hh19
2 files changed, 70 insertions, 22 deletions
diff --git a/i/marvin/src/es/es.cc b/i/marvin/src/es/es.cc
index ab73b3e..f26cf33 100644
--- a/i/marvin/src/es/es.cc
+++ b/i/marvin/src/es/es.cc
@@ -32,14 +32,23 @@
/// Constructeur
Es::Es (const Config & config)
: proto_ (*this), log_ ("Es"), lcdKeyPressed_ (-1),
- front_sensor_ (false), jackIn_ (false), colorModeBlue_ (false)
+ front_sensor_ (false), jackIn_ (false), colorModeBlue_ (false),
+ unknownColor_ (-1), 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_);
- rvbBall_[0] = rvbBall_[1] = -1;
sharps_.resize (3);
+
+ for (int compt = 0; compt < 5; compt++)
+ {
+ seenColors_[compt] = unknownColor_;
+ lastSeenColors_[compt] = unknownColor_;
+ comptSeenColors_[compt] = 0;
+ }
}
bool
@@ -79,6 +88,9 @@ Es::loadConfig (const Config & config)
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
@@ -361,16 +373,26 @@ void Es::receive(char command, const Proto::Frame & frame)
{
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, stat2))
+ if (proto_.decode (frame, "bb", stat1 /* Rear */, stat2 /* Front */))
{
log_ ("Stats RVB Ball", Log::debug) << "[" << decodeColor
(stat1) << "] [" << decodeColor (stat2) << "]";
- rvbBall_[0] = stat1;
- rvbBall_[1] = stat2;
+ updateAnalysisSensor (stat1, rearBallRVB_, thresholdBallSensors_);
+ updateAnalysisSensor (stat2, frontBallRVB_, thresholdBallSensors_);
}
break;
/* Others */
@@ -378,23 +400,14 @@ void Es::receive(char command, const Proto::Frame & frame)
if (proto_.decode (frame, "b", value))
{
// XXX Use a decallage please
- switch (value)
- {
- case 0:
- jackIn_ = colorModeBlue_ = false;
- break;
- case 1:
+ if (value & 0x01)
jackIn_ = true;
- colorModeBlue_ = false;
- break;
- case 2:
+ else
jackIn_ = false;
+ if (value & 0x02)
colorModeBlue_ = true;
- break;
- case 3:
- jackIn_ = colorModeBlue_ = true;
- break;
- }
+ else
+ colorModeBlue_ = false;
log_ ("Others", Log::debug) << "Color mode " << (colorModeBlue_ ? "Blue" : "Red")
<< (jackIn_ ? ", jack in..." : ", jack out !!!");
}
@@ -450,3 +463,25 @@ Es::decodeColor (int color)
return "unknow";
}
}
+
+/// 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];
+}
+
+/// What color do you see my lord ?
+int
+Es::colorSeen (int sensor_num)
+{
+ return seenColors_[sensor_num];
+}
diff --git a/i/marvin/src/es/es.hh b/i/marvin/src/es/es.hh
index cbf989e..600a480 100644
--- a/i/marvin/src/es/es.hh
+++ b/i/marvin/src/es/es.hh
@@ -36,6 +36,7 @@ class Config;
class Es : public Proto::Receiver
{
typedef std::vector<int> Sharps;
+
private:
// Objet Proto de communication vers la carte es
Proto proto_;
@@ -44,7 +45,7 @@ class Es : public Proto::Receiver
/// Système de log
Log log_;
/// For LCD communication
- char lcd_mess_char_[32];
+ char lcd_mess_char_[16];
/// Key pressed by the LCD Keyboard
int lcdKeyPressed_, lcdKeyStat_;
/// Frontal choc !
@@ -63,8 +64,6 @@ class Es : public Proto::Receiver
int rvbSniffMaskStat_, rvbSniffStat_;
/// RVB Sensors raw stats
int rvbBallStat_;
- /// Balls RVB
- int rvbBall_[2];
/// Sharps
Sharps sharps_;
/// Others module, jack & colour
@@ -72,8 +71,18 @@ class Es : public Proto::Receiver
int othersStat_;
/// Stat of front sensors
int rvbSniffFrontStat_;
+ int thresholdFrontSensors_, thresholdBallSensors_, thresholdHoleSensors_;
+
+
+ /// System for analyse a ball
+ int seenColors_[5], lastSeenColors_[5], comptSeenColors_[5];
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 ...
@@ -157,9 +166,13 @@ class Es : public Proto::Receiver
void rotationBarillet(int posFinal);
/// Empty everything in the barillet
void barilletEmpty (void);
+ /// What color do you see my lord ?
+ int colorSeen (int sensor_num);
private:
/// Decode a color into a string
std::string decodeColor (int color);
+ /// Update system for one sensor
+ void updateAnalysisSensor (int value, int index, int threshold);
};
#endif // es_hh