From dfcdd6007907ad86b99e8eab8b46e773ee0c4706 Mon Sep 17 00:00:00 2001 From: dufourj Date: Sun, 21 May 2006 22:15:51 +0000 Subject: ES (RVB) : - change main timer prescaler for more captures ; - add stats for ball sensors ; - analyse less senbsor at a time ; - threshold configurable for ball sensor. --- n/es-2006/src/main.c | 67 ++++++++++++++++++++++++++++-------------- n/es-2006/src/sensor_rvb.c | 73 ++++++++++++++++++++++++++++++++++++---------- n/es-2006/src/sensor_rvb.h | 6 ++++ n/es-2006/src/sniff_rvb.c | 28 +++++++++--------- n/es-2006/src/sniff_rvb.h | 7 ++--- n/es-2006/src/timer_main.h | 2 +- 6 files changed, 127 insertions(+), 56 deletions(-) diff --git a/n/es-2006/src/main.c b/n/es-2006/src/main.c index bde33eb..5d79a4f 100644 --- a/n/es-2006/src/main.c +++ b/n/es-2006/src/main.c @@ -44,6 +44,9 @@ 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]; +/* Statistics for RVB ball sensors. */ +uint8_t sniff_ball_stat_enable[2], sniff_ball_stats[2]; + /** Call when we receive some data from proto (uart) */ void proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) @@ -68,8 +71,9 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) sensor_rvb_config (args[0], temp_16b); break; /* RVB sniff configuration */ - case c ('x', 1): - sniff_rvb_config (args[0]); + case c ('x', 5): + sniff_rvb_config (args[0], v8_to_v16 (args[1], args[2]), + v8_to_v16 (args[3], args[4])); break; /* RVB sensors stats */ case c ('S', 3): @@ -94,23 +98,31 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) sniff_rvb_stat_enable[compt] = sniff_rvb_stats[compt] = args[2]; break; + /* Print stats for ball */ + case c ('B', 2): + for (compt = 0; compt < 2; compt++) + if (args[0] & _BV(compt)) + sniff_ball_stats[compt] = sniff_ball_stat_enable[compt] = args[1]; + break; + case c ('u', 1): + sensor_rvb_upper_sensors (args[0]); + break; + /* contact */ + case c ('i', 0 ): + proto_send1b ('i', others_contact()); + break; + /* jack */ + case c ('j', 0 ): + proto_send1b ('j', others_jack()); + break; /* color mode game */ - case c ('c', 0 ): - proto_send1b('c', others_selectcoul()); + case c ('k', 0 ): + proto_send1b ('k', others_selectcoul()); break; /* Communication with the servo motor */ case c ('m', 2): servo_motor_set_pos (args[0], args[1]); break; -// /* contact */ -// case c ('TTTTTTTTTTt', 0 ): -// proto_send1b('t', others_contact()); -// break; -// -// /* jack */ -// case c ('j', 0 ): -// proto_send1b('j', others_jack()); -// break; /* tests primaires pour le barillet */ @@ -179,10 +191,6 @@ 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'); @@ -201,7 +209,8 @@ main (void) 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][1], + sensor_rvb_values[compt][2], sensor_rvb_values[compt][3]); } @@ -213,9 +222,21 @@ main (void) sniff_rvb_stats[compt] = sniff_rvb_stat_enable[compt]; proto_send2b ('A', compt, sniff_rvb_analysis_color (compt, RVB_SNIFF_ALL_COLORS)); } + + /* Print stats for ball */ + for (compt = 0; compt < 2; compt++) + if (sniff_ball_stat_enable[compt] && !--sniff_ball_stats[compt]) + { + /* Re init stats system for this sensor */ + sniff_ball_stats[compt] = sniff_ball_stat_enable[compt]; + proto_send2b ('B', compt, sniff_rvb_analysis_ball (compt + 7)); + } + /* Update all the asserv pin comunication */ sensor_rvb_update_asserv_pins (); + // FIXME, if we can put some of this values into the eeprom, it will + // be better... if (init_rvb > 0) { init_rvb--; @@ -229,13 +250,15 @@ main (void) } /* gestion du barilet */ - sequenceur_barillet(); - - /* Update RVB sensors data. */ - sensor_rvb_start_capture (); + sequenceur_barillet (); /* Get data for serial port */ while (uart0_poll ()) proto_accept (uart0_getc ()); + + /* Update RVB sensors data. + * I want this to be done as later as possible, and espceically after + * commands has been parsed ! */ + sensor_rvb_start_capture (); } } diff --git a/n/es-2006/src/sensor_rvb.c b/n/es-2006/src/sensor_rvb.c index 76e956a..fae1c27 100644 --- a/n/es-2006/src/sensor_rvb.c +++ b/n/es-2006/src/sensor_rvb.c @@ -78,6 +78,9 @@ 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_; +/** Barillet sensors are enabled ? */ +volatile uint8_t sensor_rvb_barillet_flag_; +volatile uint8_t sensor_rvb_low_sensor_flag_; /** Update everything for you. */ inline void sensor_rvb_update (void); @@ -109,7 +112,7 @@ sensor_rvb_color_select (uint8_t sensor_rvb_color) } /** Enable a RVB sensor. */ -inline uint8_t +uint8_t sensor_rvb_sensor_select (uint8_t sensor_rvb_num) { static uint8_t sensor_cur = 0; @@ -122,22 +125,44 @@ sensor_rvb_sensor_select (uint8_t sensor_rvb_num) PORTC = ~_BV(sensor_rvb_num); break; case 5: - switch (sensor_cur) + /* If we also want the other sensors */ + if (sensor_rvb_low_sensor_flag_) { - case 0: - case 1: - case 2: - PORTC = ~_BV(sensor_rvb_num + sensor_cur); - break; - case 3: - PORTD &= ~_BV(6); - break; - case 4: - PORTC = ~_BV(0); - break; + /* Normal behaviour */ + PORTC = ~_BV(5); } - if (++sensor_cur == 5) - sensor_cur = 0; + else + { + /* Alternatively one sensor of the following */ + if (sensor_cur == 0) + PORTC = ~_BV(5); + else + PORTC = 0x7F; + if (++sensor_cur == 2) + sensor_cur = 0; + } + break; + case 6: + if (!sensor_rvb_low_sensor_flag_) + { + /* High for disable */ + PORTC = 0xff; + PORTD |= _BV(6); + /* Wait a little */ + utils_delay_ns (RVB_SENSOR_DELAY_IO); + return 0; + } + else + PORTC = ~_BV(6); + break; + case 7: + PORTC = 0x7F; + break; + case 8: + PORTD &= ~_BV(6); + break; + case 9: + PORTC = ~_BV(0); break; default: /* High for disable */ @@ -345,3 +370,21 @@ sensor_rvb_update_asserv_pins (void) // which pins we should update. } +/** Enable/disable capture for black & white sensors (the ones inside the + * barillet). + */ +void +sensor_rvb_upper_sensors (uint8_t enable) +{ + if (enable) + sensor_rvb_low_sensor_flag_ = 1; + else + { + /* Invalidate all capture */ + // XXX Good idea ? + sensor_rvb_low_sensor_flag_ = 0; + sensor_rvb_values[8][0] = + sensor_rvb_values[7][0] = RVB_INVALID_CAPTURE; + } +} + diff --git a/n/es-2006/src/sensor_rvb.h b/n/es-2006/src/sensor_rvb.h index a46cfa4..8e0e73f 100644 --- a/n/es-2006/src/sensor_rvb.h +++ b/n/es-2006/src/sensor_rvb.h @@ -83,4 +83,10 @@ void sensor_rvb_config (uint8_t false_ic, uint16_t max_ov); * sensors. */ void sensor_rvb_update_asserv_pins (void); +/** Enable/disable capture for black & white sensors (the ones inside the + * barillet). + * - enable : 1 if you want this sensor to be captured, 0 otherwise. + */ +void sensor_rvb_upper_sensors (uint8_t enable); + #endif // sensor_rvb_h diff --git a/n/es-2006/src/sniff_rvb.c b/n/es-2006/src/sniff_rvb.c index 8c5aef6..fa48498 100644 --- a/n/es-2006/src/sniff_rvb.c +++ b/n/es-2006/src/sniff_rvb.c @@ -25,10 +25,7 @@ #include "sniff_rvb.h" #include "io.h" -#include "string.h" #include "common.h" -#include "modules/proto/proto.h" -#include "modules/utils/byte.h" #include "sensor_rvb.h" @@ -38,20 +35,23 @@ uint16_t sniff_rvb_reference_color[RVB_MAX_SENSOR][RVB_SNIFF_MAX_INDEX]; uint16_t sniff_rvb_reference_color_min[RVB_MAX_SENSOR][RVB_SNIFF_MAX_INDEX]; uint16_t sniff_rvb_reference_color_max[RVB_MAX_SENSOR][RVB_SNIFF_MAX_INDEX]; /** Ratio for the computing the min/max value from the reference color. */ -uint8_t sniff_rvb_ref_ratio = 3; +uint8_t sniff_rvb_ref_ratio_ = 3; /* test with new robot clothes */ /* ball black/white detection */ -uint16_t green_limit = 7000; -uint16_t clear_limit = 8000; - +uint16_t green_limit_ = 2800; +uint16_t clear_limit_ = 3200; /** Configure the sensor analysis system. */ void -sniff_rvb_config (uint8_t ref_ratio) +sniff_rvb_config (uint8_t ref_ratio, uint16_t green_limit, + uint16_t clear_limit) { - sniff_rvb_ref_ratio = ref_ratio; + sniff_rvb_ref_ratio_ = ref_ratio; + green_limit_ = green_limit; + clear_limit_ = clear_limit; } + /** Try to reference the sensor_num sensor into the reference color. */ inline uint8_t sniff_rvb_try_reference (uint8_t sensor_num, uint16_t @@ -81,10 +81,10 @@ sniff_rvb_try_reference (uint8_t sensor_num, uint16_t /* test "barillet" black/white */ uint8_t -sniff_rvb_ball (uint8_t sensor) +sniff_rvb_analysis_ball (uint8_t sensor) { - if ((sensor_rvb_values[sensor][RVB_INDEX_GREEN] > green_limit) - && (sensor_rvb_values[sensor][RVB_INDEX_CLEAR] > clear_limit)) + if ((sensor_rvb_values[sensor][RVB_INDEX_GREEN] > green_limit_) + && (sensor_rvb_values[sensor][RVB_INDEX_CLEAR] > clear_limit_)) return RVB_SNIFF_BLACK; else return RVB_SNIFF_WHITE; @@ -157,10 +157,10 @@ sniff_rvb_set_ref_color_green (uint8_t sensor) { sniff_rvb_reference_color_min[sensor][compt] = sniff_rvb_reference_color[sensor][compt] - - (sniff_rvb_reference_color[sensor][compt] / sniff_rvb_ref_ratio); + (sniff_rvb_reference_color[sensor][compt] / sniff_rvb_ref_ratio_); sniff_rvb_reference_color_max[sensor][compt] = sniff_rvb_reference_color[sensor][compt] + - (sniff_rvb_reference_color[sensor][compt] / sniff_rvb_ref_ratio); + (sniff_rvb_reference_color[sensor][compt] / sniff_rvb_ref_ratio_); } } diff --git a/n/es-2006/src/sniff_rvb.h b/n/es-2006/src/sniff_rvb.h index b318485..bf988b4 100644 --- a/n/es-2006/src/sniff_rvb.h +++ b/n/es-2006/src/sniff_rvb.h @@ -27,7 +27,6 @@ #include "io.h" #include "common.h" -#include "sensor_rvb.h" /** Analysis sensors and determine the colors. */ @@ -51,7 +50,6 @@ */ void sniff_rvb_set_ref_color (uint8_t sensor, uint8_t color); - /** Analysis a color : * - sensor : the sensor number to analysis ; * - mode : for futur use. @@ -62,11 +60,12 @@ uint8_t sniff_rvb_analysis_color (uint8_t sensor, uint8_t mode); /* Test "barillet" black/white * - sensor : the sensor number to analysis ; */ -uint8_t sniff_rvb_ball (uint8_t sensor); +uint8_t sniff_rvb_analysis_ball (uint8_t sensor); /** Configure the sensor analysis system. * - ref_ratio : ratio for the reference color. */ -void sniff_rvb_config (uint8_t ref_ratio); +void sniff_rvb_config (uint8_t ref_ratio, uint16_t green_limit, + uint16_t clear_limit); #endif // sniff_rvb_h diff --git a/n/es-2006/src/timer_main.h b/n/es-2006/src/timer_main.h index 458db2e..a130683 100644 --- a/n/es-2006/src/timer_main.h +++ b/n/es-2006/src/timer_main.h @@ -31,7 +31,7 @@ inline void timer_main_init (void) { TCCR0 = regv (FOC0, WGM00, COM01, COM0, WGM01, CS02, CS01, CS00, - 0, 0, 0, 0, 0, 1, 1, 0); + 0, 0, 0, 0, 0, 1, 0, 1); /* Fov = F_io / (prescaler * (TOP + 1)) * TOP = 0xff * prescaler = 256 -- cgit v1.2.3