From 28243d592481d45eaa8c077684c164bc4fb76c9b Mon Sep 17 00:00:00 2001 From: dufourj Date: Sat, 13 May 2006 21:59:17 +0000 Subject: ES : - calibration du vert - detection de couleur green/other marche - debut de detection du bleu/rouge () () (o o) (_)(_) --- n/es-2006/src/Makefile | 2 +- n/es-2006/src/main.c | 80 ++++++++++++++++++++--------- n/es-2006/src/sensor_rvb.c | 125 ++++++++++++++------------------------------- n/es-2006/src/sensor_rvb.h | 24 ++------- 4 files changed, 99 insertions(+), 132 deletions(-) diff --git a/n/es-2006/src/Makefile b/n/es-2006/src/Makefile index 460d20d..ef1aba9 100644 --- a/n/es-2006/src/Makefile +++ b/n/es-2006/src/Makefile @@ -1,6 +1,6 @@ BASE = ../../avr PROGS = es -es_SOURCES = main.c sensor_rvb.c timer_1.c barillet.c +es_SOURCES = main.c sensor_rvb.c sniff_rvb.c timer_1.c barillet.c MODULES = proto uart utils CONFIGFILE = avrconfig.h # atmega8, atmega8535, atmega128... diff --git a/n/es-2006/src/main.c b/n/es-2006/src/main.c index 07a4a28..078fc19 100644 --- a/n/es-2006/src/main.c +++ b/n/es-2006/src/main.c @@ -32,28 +32,42 @@ #include "timer_main.h" /* main timer */ #include "sensor_rvb.h" /* RVB sensors management */ -#include "timer_1.h" /* timer/counter 1 */ +#include "sniff_rvb.h" /* RVB sensors analysis */ +#include "timer_1.h" /* timer/counter 1 */ +#include "others.h" /* define game color mode + jack + frontal sensor */ -#include "others.h" /* define game color mode + jack + frontal sensor */ /* Statistics for RVB sensors */ -uint8_t sensor_rvb_stat_enable[RVB_MAX_SENSOR] = { 0 }; -uint8_t sensor_rvb_stats[RVB_MAX_SENSOR] = { 0 }; +uint8_t sensor_rvb_stat_enable[RVB_MAX_SENSOR], sensor_rvb_stats[RVB_MAX_SENSOR]; +/* Statistics for RVB analysis. */ +uint8_t sniff_rvb_stat_enable[RVB_MAX_SENSOR], sniff_rvb_stats[RVB_MAX_SENSOR]; /** Call when we receive some data from proto (uart) */ void proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) { - uint8_t compt; - uint16_t temp_16b; /* This macro combine command and size in one integer. */ #define c(cmd, size) (cmd << 8 | size) + + uint8_t compt; + uint16_t temp_16b; + switch (c (cmd, size)) { /* Reset */ case c ('z', 0): utils_reset (); break; + + /*** RVB ***/ + /* RVB sensors configuration */ + case c ('p', 4): + sensor_rvb_config (args[0], args[1], args[2]); + break; + /* RVB sniff configuration */ + case c ('x', 1): + sniff_rvb_config (args[0]); + break; /* RVB sensors stats */ case c ('S', 3): temp_16b = v8_to_v16 (args[0], args[1]); @@ -64,24 +78,27 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) break; /* Set threshold for the differents colors */ case c ('s', 9): - sensor_rvb_set_threshold + sniff_rvb_set_threshold (args[0], v8_to_v16 (args[1], args[2]), v8_to_v16 (args[3], args[4]), v8_to_v16 (args[5], args[6]), v8_to_v16 (args[7], args[8])); break; - /* RVB sensors configuration */ - case c ('R', 3): - sensor_rvb_config (args[0], args[1], args[2]); - break; /* Set current color as reference */ - case c ('r', 0): - sensor_rvb_set_ref_color (); + case c ('r', 1): + sniff_rvb_set_ref_color (args[0]); + break; + /* Print stats if the sensor is seen */ + case c ('A', 3): + temp_16b = v8_to_v16 (args[0], args[1]); + for (compt = 0; compt < RVB_MAX_SENSOR; compt++) + if (temp_16b & _BV(compt)) + sniff_rvb_stat_enable[compt] = sniff_rvb_stats[compt] = + args[2]; break; /* color mode game */ case c ('c', 0 ): proto_send1b('c', others_selectcoul()); break; - // /* contact */ // case c ('t', 0 ): // proto_send1b('t', others_contact()); @@ -121,27 +138,44 @@ main (void) /* Enable interrupts */ sei (); + // XXX + sniff_rvb_set_threshold (RVB_SNIFF_RED, -100, -85, 725, 0); + sniff_rvb_set_threshold (RVB_SNIFF_BLUE, 650, -140, -1050, -950); + /* We are ready ! */ proto_send0 ('z'); while (1) { /* Wait 4.44 ms */ + // FIXME Try to put this down and make some test before if it + // possible. timer_main_wait (); - + /* RVB Sensors stats */ for (compt = 0; compt < RVB_MAX_SENSOR; compt++) if (sensor_rvb_stat_enable[compt] && !--sensor_rvb_stats[compt]) - { - /* Re init stats system for this sensor */ - sensor_rvb_stats[compt] = sensor_rvb_stat_enable[compt]; - // FIXME useless 16 bit value. - proto_send5w ('S', compt, sensor_rvb_values[compt][0], - sensor_rvb_values[compt][1], sensor_rvb_values[compt][2], - sensor_rvb_values[compt][3]); - } + { + /* Re init stats system for this sensor */ + sensor_rvb_stats[compt] = sensor_rvb_stat_enable[compt]; + // FIXME useless 16 bit value. + proto_send5w ('S', compt, sensor_rvb_values[compt][0], + sensor_rvb_values[compt][1], sensor_rvb_values[compt][2], + sensor_rvb_values[compt][3]); + } + + /* RVB Sensors analysis stats */ + for (compt = 0; compt < RVB_MAX_SENSOR; compt++) + if (sniff_rvb_stat_enable[compt] && !--sniff_rvb_stats[compt]) + { + /* Re init stats system for this sensor */ + sniff_rvb_stats[compt] = sniff_rvb_stat_enable[compt]; + proto_send2b ('A', compt, sniff_rvb_analysis_color (compt, RVB_SNIFF_ALL_COLORS)); + } /* Update RVB sensors data. */ sensor_rvb_start_capture (); + /* Update all the asserv pin comunication */ +// sensor_rvb_update_asserv_pins (); /* Get data for serial port */ while (uart0_poll ()) diff --git a/n/es-2006/src/sensor_rvb.c b/n/es-2006/src/sensor_rvb.c index a8ccf5a..5d86349 100644 --- a/n/es-2006/src/sensor_rvb.c +++ b/n/es-2006/src/sensor_rvb.c @@ -29,6 +29,7 @@ #include "modules/utils/utils.h" /* regv */ #include "modules/proto/proto.h" +#include "sniff_rvb.h" /** * Somes defines. **/ @@ -54,11 +55,12 @@ #define RVB_STATE_NEXT_COLOR 4 /* Selecting another color */ #define RVB_STATE_WATCH 5 /* For debuging */ + /*** Config ***/ /** Max input capture before considering we can start the real capture. */ uint8_t sensor_rvb_conf_max_false_ic_; /** Max overflow of the timer 1 before thinking the sensor is HS. */ -uint8_t sensor_rvb_conf_max_ov_; +uint16_t sensor_rvb_conf_max_ov_; /** Wait time between IO change in ns. */ uint8_t sensor_rvb_conf_delay_; /** RVB sensor state flag. */ @@ -75,10 +77,6 @@ volatile uint8_t sensor_rvb_overflow_count_; volatile uint16_t sensor_rvb_values[RVB_MAX_SENSOR][4]; /** Count the number of IC before starting a good capture. */ uint8_t sensor_rvb_ic_count_; -/** Reference color. */ -uint16_t sensor_rvb_reference_color[4] = { 0 }; -/** Threshold table data. */ -uint16_t sensor_rvb_threshold_table[4][4] = { { 0 } }; /** Update everything for you. */ void sensor_rvb_update (void); @@ -143,83 +141,6 @@ sensor_rvb_sensor_select (uint8_t sensor_rvb_num) return 1; } -/** Try to reference the sensor_num sensor into the reference color. */ -inline uint8_t -sensor_rvb_try_reference (uint8_t sensor_num) -{ - uint8_t compt; - - /* If sensor is valid */ - if (sensor_rvb_values[sensor_num][0]) - { - /* For each color add this value by averrage */ - for (compt = 0; compt < 4; compt++) - { - sensor_rvb_reference_color[compt] = - (sensor_rvb_values[sensor_num][compt] - + sensor_rvb_reference_color[compt]) / 2; - } - return 1; - } - return 0; -} - - -/** Analysis a color */ -inline uint8_t -sensor_rvb_analysis_color (uint8_t sensor, uint8_t mode) -{ - return 0; -} - -/** Set the current color seen by some sensors as the reference color. */ -void -sensor_rvb_set_ref_color (void) -{ - /* The most significant sensor to uses are 7 & 8 for the moment */ - if (sensor_rvb_try_reference (7) || sensor_rvb_try_reference (8)) - // XXX Put this outside of this file ? - proto_send4w ('r', - sensor_rvb_reference_color[0], - sensor_rvb_reference_color[1], - sensor_rvb_reference_color[2], - sensor_rvb_reference_color[3]); -} - -/** Set the threshold of the differents colors. */ -void -sensor_rvb_set_threshold (uint8_t color, uint16_t red_ts, uint16_t blue_ts, - uint16_t clear_ts, uint16_t green_ts) -{ - switch (color) - { - case RVB_INDEX_RED: - case RVB_INDEX_BLUE: - case RVB_INDEX_CLEAR: - case RVB_INDEX_GREEN: - sensor_rvb_threshold_table[color][0] = red_ts; - sensor_rvb_threshold_table[color][1] = blue_ts; - sensor_rvb_threshold_table[color][2] = clear_ts; - sensor_rvb_threshold_table[color][3] = green_ts; - break; - } -} - -/** Update pins connected to the asserv AVR with the colors seen by the - * sensors. */ -void -sensor_rvb_update_asserv_pins (void) -{ - // TODO: - // - called the desired sensors with sensor_rvb_analysis_color ; - // - choose with Ni ; - // - do we need to update all the pins ? - // - this functions have to be hightly configurable : for example, choose - // if we can select frequency of update ; - // - manage a case for knowing the mode of the robot in order to select - // which pins we should update. -} - /** Initialisation of the RVB sensors module. */ void sensor_rvb_init (void) @@ -243,7 +164,7 @@ sensor_rvb_init (void) /** Configure some internal variables. */ void -sensor_rvb_config (uint8_t false_ic, uint8_t max_ov, uint8_t latency) +sensor_rvb_config (uint8_t false_ic, uint16_t max_ov, uint8_t latency) { sensor_rvb_conf_max_false_ic_ = false_ic; sensor_rvb_conf_max_ov_ = max_ov; @@ -284,7 +205,7 @@ sensor_rvb_update (void) { sensor_rvb_state_ = RVB_STATE_WAIT_IC; /* Ensure we have no pending interrupt for IC1. */ - TIFR = _BV (ICF1); +// TIFR = _BV (ICF1); /* Enable interrupt for IC1 */ TIMSK |= _BV (TICIE1); } @@ -300,7 +221,7 @@ sensor_rvb_update (void) TIMSK &= ~_BV (TICIE1); TIMSK &= ~_BV (TOIE1); /* Ensure we have no pending interrupt for IC1. */ - TIFR = _BV (ICF1); +// TIFR = _BV (ICF1); /* Finish ! Go to sleep */ sensor_rvb_state_ = RVB_STATE_SLEEP; return; @@ -345,6 +266,7 @@ SIGNAL (SIG_OVERFLOW1) /** Interrupt on falling edge for Input Capture 1. */ SIGNAL (SIG_INPUT_CAPTURE1) { + uint16_t temp; switch (sensor_rvb_state_) { case RVB_STATE_WAIT_IC: @@ -359,14 +281,16 @@ SIGNAL (SIG_INPUT_CAPTURE1) case RVB_STATE_START_CAPTURE: /* Save capture start */ sensor_rvb_value_ = ICR1; + /* Reset overflow_count */ sensor_rvb_overflow_count_ = 0; /* New capture */ sensor_rvb_state_ = RVB_STATE_STOP_CAPTURE; break; case RVB_STATE_STOP_CAPTURE: - /* Save */ - // XXX Check this please - sensor_rvb_values[sensor_rvb_number_ - 1][sensor_rvb_color_] = ICR1 + + /* Save it earlier ! */ + temp = ICR1; + /* Compute value */ + sensor_rvb_values[sensor_rvb_number_ - 1][sensor_rvb_color_] = temp + sensor_rvb_overflow_count_ * TC1_TOP - sensor_rvb_value_; /* Disable IC interrupt */ TIMSK &= ~_BV (TICIE1); @@ -377,3 +301,28 @@ SIGNAL (SIG_INPUT_CAPTURE1) } } +/** Update pins connected to the asserv AVR with the colors seen by the + * sensors. */ +void +sensor_rvb_update_asserv_pins (void) +{ + uint8_t compt; + + for (compt = 4; compt < 8; compt++) + { + if (sniff_rvb_analysis_color (compt, 0)) + PORTA |= _BV (compt); + else + PORTA &= ~_BV (compt); + } + + // TODO: + // - called the desired sensors with sensor_rvb_analysis_color ; + // - choose with Ni ; + // - do we need to update all the pins ? + // - this functions have to be hightly configurable : for example, choose + // if we can select frequency of update ; + // - manage a case for knowing the mode of the robot in order to select + // which pins we should update. +} + diff --git a/n/es-2006/src/sensor_rvb.h b/n/es-2006/src/sensor_rvb.h index ef53706..787dc98 100644 --- a/n/es-2006/src/sensor_rvb.h +++ b/n/es-2006/src/sensor_rvb.h @@ -56,10 +56,12 @@ #define RVB_INDEX_BLUE 1 #define RVB_INDEX_CLEAR 2 #define RVB_INDEX_GREEN 3 +/** Define the number of color the sensor can manages. */ +#define RVB_MAX_INDEX 4 /** Results table for 9 RVB sensors (RVCB) : * Values are in the folowing order : Red, Blue, Clear, Green. */ -extern volatile uint16_t sensor_rvb_values[RVB_MAX_SENSOR][4]; +extern volatile uint16_t sensor_rvb_values[RVB_MAX_SENSOR][RVB_MAX_INDEX]; /** Initialisation of the RVB sensors module. */ void sensor_rvb_init (void); @@ -75,25 +77,7 @@ void sensor_rvb_start_capture (void); * - latency : delay between change for sensor. Must be more than 100ns. This * setting is in ns. */ -void sensor_rvb_config (uint8_t false_ic, uint8_t max_ov, uint8_t latency); - -/** Set the current color seen by some sensors as the reference color. - * It must be called at the begining of the match for setting the green value. - */ -void sensor_rvb_set_ref_color (void); - -/** Set the threshold of the differents colors. - * There are used for the algorithm that distinguish colors. - */ -void sensor_rvb_set_threshold (uint8_t color, uint16_t red_ts, uint16_t - blue_ts, uint16_t clear_ts, uint16_t green_ts); - -/** Analysis a color : - * - sensor : the sensor number to analysis ; - * - mode : for futur use. - * return the color number. - */ -uint8_t sensor_rvb_analysis_color (uint8_t sensor, uint8_t mode); +void sensor_rvb_config (uint8_t false_ic, uint16_t max_ov, uint8_t latency); /** Update pins connected to the asserv AVR with the colors seen by the * sensors. */ -- cgit v1.2.3