From c60d987228c653e19ce9f9386b3fef7fc28cb396 Mon Sep 17 00:00:00 2001 From: dufourj Date: Wed, 24 May 2006 23:24:17 +0000 Subject: Es : - analysis system for seen color. ES : - add stat function for hole RVB sensor. --- i/marvin/src/es/es.cc | 73 +++++++++++++++++++++++++++++++++++++-------------- i/marvin/src/es/es.hh | 19 +++++++++++--- 2 files changed, 70 insertions(+), 22 deletions(-) (limited to 'i/marvin/src/es') 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 ("es.rvb_ball_stat"); othersStat_ = config.get ("es.others_stat"); lcdKeyStat_ = config.get ("es.lcd_key_stat"); + thresholdFrontSensors_ = config.get ("es.threshold_front_sensors"); + thresholdBallSensors_ = config.get ("es.threshold_ball_sensors"); + thresholdHoleSensors_ = config.get ("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 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 -- cgit v1.2.3