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/runtime/rc/config | 6 ++++ i/marvin/src/es/es.cc | 73 ++++++++++++++++++++++++++++++++++------------ i/marvin/src/es/es.hh | 19 ++++++++++-- n/es-2006/src/main.c | 18 ++++++++++++ 4 files changed, 94 insertions(+), 22 deletions(-) diff --git a/i/marvin/runtime/rc/config b/i/marvin/runtime/rc/config index d73f29b..b420038 100644 --- a/i/marvin/runtime/rc/config +++ b/i/marvin/runtime/rc/config @@ -56,6 +56,12 @@ es.others_stat = 0 es.lcd_key_stat = 0 # Print stat of front sensors es.rvb_sniff_front_stat = 0 +# Threshold of front sensors at the bottom +es.threshold_front_sensors = 4 +# Threshold of ball sensors +es.threshold_ball_sensors = 4 +# Threshold of hole sensor +es.threshold_hole_sensors = 4 ### Artificial Intelligency ! # Time of match in milliseconds ! 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 diff --git a/n/es-2006/src/main.c b/n/es-2006/src/main.c index 172c385..ca3fec1 100644 --- a/n/es-2006/src/main.c +++ b/n/es-2006/src/main.c @@ -58,6 +58,8 @@ uint8_t others_jackcolor_stat_enable, others_jackcolor_stats; /* Stat for RVB front sensors */ uint8_t sniff_rvb_front_enable, sniff_rvb_front_stats; +/* Stat for RVB hole sensor */ +uint8_t sniff_rvb_hole_enable, sniff_rvb_hole_stats; /* XXX Debug stats */ uint8_t main_stats[4]; @@ -151,6 +153,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) case c ('C', 1): sniff_rvb_front_enable = sniff_rvb_front_stats = args[0]; break; + /* RVB Hole sensor */ + case c ('D', 1): + sniff_rvb_hole_enable = sniff_rvb_hole_stats = args[0]; + break; /* Enable all sensors */ case c ('u', 1): sensor_rvb_upper_sensors (args[0]); @@ -354,6 +360,18 @@ main (void) else sniff_rvb_front_stats = 1; } + /* RVB Hole */ + if (sniff_rvb_hole_enable && !--sniff_rvb_hole_stats) + { + if (sensor_rvb_state_sleeeping ()) + { + /* Re init stats system for this sensor */ + sniff_rvb_hole_stats = sniff_rvb_hole_enable; + proto_send1b ('D', sniff_rvb_analysis_ball (5)); + } + else + sniff_rvb_hole_stats = 1; + } main_stats[1] = TCNT0; -- cgit v1.2.3