From fea11424132ba8a904788fbc4216a65e2e86413a Mon Sep 17 00:00:00 2001 From: dufourj Date: Sun, 21 May 2006 11:46:26 +0000 Subject: ES (RVB) : - initialise la couleur de reference au reste de la carte es ; - certains capteurs sont mis à jour moins souvent ; - meilleur gestion des interruptions et du calcul des valeurs des capteurs ; - gestion des saisies erroné et la mise à jour des pattes de l'asserv. --- n/es-2006/src/main.c | 17 +++++++++-- n/es-2006/src/sensor_rvb.c | 71 +++++++++++++++++++++++++++++----------------- n/es-2006/src/sensor_rvb.h | 2 ++ n/es-2006/src/sniff_rvb.c | 7 +++-- 4 files changed, 66 insertions(+), 31 deletions(-) (limited to 'n') diff --git a/n/es-2006/src/main.c b/n/es-2006/src/main.c index fdab030..bde33eb 100644 --- a/n/es-2006/src/main.c +++ b/n/es-2006/src/main.c @@ -158,6 +158,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) int main (void) { + uint8_t init_rvb = 200; uint8_t compt; /* Serial port */ @@ -214,12 +215,24 @@ main (void) } /* Update all the asserv pin comunication */ sensor_rvb_update_asserv_pins (); - /* Update RVB sensors data. */ - sensor_rvb_start_capture (); + + if (init_rvb > 0) + { + init_rvb--; + if ((init_rvb == 1) || (init_rvb == 2)) + { + sniff_rvb_set_ref_color (0, 0); + sniff_rvb_set_ref_color (1, 0); + sniff_rvb_set_ref_color (2, 0); + sniff_rvb_set_ref_color (3, 0); + } + } /* gestion du barilet */ sequenceur_barillet(); + /* Update RVB sensors data. */ + sensor_rvb_start_capture (); /* 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 5a4de2a..76e956a 100644 --- a/n/es-2006/src/sensor_rvb.c +++ b/n/es-2006/src/sensor_rvb.c @@ -112,22 +112,32 @@ sensor_rvb_color_select (uint8_t sensor_rvb_color) inline uint8_t sensor_rvb_sensor_select (uint8_t sensor_rvb_num) { + static uint8_t sensor_cur = 0; switch (sensor_rvb_num) { case 1: case 2: case 3: case 4: - case 5: - case 6: - case 7: PORTC = ~_BV(sensor_rvb_num); break; - case 8: - PORTD &= ~_BV(6); - break; - case 9: - PORTC = ~_BV(0); + case 5: + switch (sensor_cur) + { + 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; + } + if (++sensor_cur == 5) + sensor_cur = 0; break; default: /* High for disable */ @@ -182,6 +192,9 @@ sensor_rvb_start_capture (void) sensor_rvb_number_ = 0; /* Select it and launch capture */ sensor_rvb_update (); + /* Ensure we have no pending interrupt for IC1. */ + TIFR = _BV (ICF1); + TIFR = _BV (TOV1); /* Enable interrupt for IC1 and counter overflow */ TIMSK |= _BV (TICIE1); TIMSK |= _BV (TOIE1); @@ -189,8 +202,7 @@ sensor_rvb_start_capture (void) } /** Update everything for you. */ -// TODO, common part, clean TIFR, remove inline ? -inline void +void sensor_rvb_update (void) { if (sensor_rvb_state_ == RVB_STATE_NEXT_COLOR) @@ -205,7 +217,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); } @@ -220,8 +232,6 @@ sensor_rvb_update (void) /* Disable IC1 and TC1 overflow interrupts */ TIMSK &= ~_BV (TICIE1); TIMSK &= ~_BV (TOIE1); - /* Ensure we have no pending interrupt for IC1. */ -// TIFR = _BV (ICF1); /* Finish ! Go to sleep */ sensor_rvb_state_ = RVB_STATE_SLEEP; return; @@ -230,8 +240,8 @@ sensor_rvb_update (void) sensor_rvb_color_select (sensor_rvb_color_ = 0); sensor_rvb_state_ = RVB_STATE_WAIT_IC; sensor_rvb_overflow_count_ = 0; -// /* Ensure we have no pending interrupt for IC1. */ -// TIFR = _BV (ICF1); + /* Ensure we have no pending interrupt for IC1. */ + TIFR = _BV (ICF1); /* Enable interrupt for IC1 */ TIMSK |= _BV (TICIE1); } @@ -252,7 +262,7 @@ SIGNAL (SIG_OVERFLOW1) if (++sensor_rvb_overflow_count_ >= sensor_rvb_conf_max_ov_) { /* Invalidate the sensor */ - sensor_rvb_values[sensor_rvb_number_ - 1][0] = 0; + sensor_rvb_values[sensor_rvb_number_ - 1][0] = RVB_INVALID_CAPTURE; /* Disable IC interrupt */ TIMSK &= ~_BV (TICIE1); /* Ask for next sensor */ @@ -266,7 +276,8 @@ SIGNAL (SIG_OVERFLOW1) /** Interrupt on falling edge for Input Capture 1. */ SIGNAL (SIG_INPUT_CAPTURE1) { - uint16_t temp; + /* Save it earlier ! */ + uint16_t ic1_cache = ICR1; switch (sensor_rvb_state_) { case RVB_STATE_WAIT_IC: @@ -279,19 +290,22 @@ SIGNAL (SIG_INPUT_CAPTURE1) sensor_rvb_state_ = RVB_STATE_START_CAPTURE; break; case RVB_STATE_START_CAPTURE: + /* Manage case when overflow occured before IC but we are called first + * */ + if (!(ic1_cache & (TC1_TOP & ~(TC1_TOP >> 1)))) + TIFR = _BV (TOV1); /* Save capture start */ - sensor_rvb_value_ = ICR1; + sensor_rvb_value_ = ic1_cache; /* Reset overflow_count */ sensor_rvb_overflow_count_ = 0; /* New capture */ sensor_rvb_state_ = RVB_STATE_STOP_CAPTURE; break; case RVB_STATE_STOP_CAPTURE: - /* 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_; + sensor_rvb_values[sensor_rvb_number_ - 1][sensor_rvb_color_] = + ic1_cache + sensor_rvb_overflow_count_ * (TC1_TOP + 1) - + sensor_rvb_value_; /* Disable IC interrupt */ TIMSK &= ~_BV (TICIE1); sensor_rvb_state_ = RVB_STATE_NEXT_COLOR; @@ -310,10 +324,15 @@ sensor_rvb_update_asserv_pins (void) for (compt = 0; compt < 4; compt++) { - if (sniff_rvb_analysis_color (compt, 0)) - PORTA |= _BV (compt + 4); - else - PORTA &= ~_BV (compt + 4); + /* Is sensor capture valid */ + if (sensor_rvb_values[compt][0] < RVB_INVALID_CAPTURE) + { + if (sniff_rvb_analysis_color (compt, RVB_SNIFF_ONLY_GREEN) + != RVB_SNIFF_GREEN) + PORTA |= _BV (compt + 4); + else + PORTA &= ~_BV (compt + 4); + } } // TODO: diff --git a/n/es-2006/src/sensor_rvb.h b/n/es-2006/src/sensor_rvb.h index c863aeb..a46cfa4 100644 --- a/n/es-2006/src/sensor_rvb.h +++ b/n/es-2006/src/sensor_rvb.h @@ -58,6 +58,8 @@ #define RVB_INDEX_GREEN 3 /** Define the number of color the sensor can manages. */ #define RVB_MAX_INDEX 4 +/** Is it an invalid value for sensor */ +#define RVB_INVALID_CAPTURE 0xE000 /** Results table for 9 RVB sensors (RVCB) : * Values are in the folowing order : Red, Blue, Clear, Green. */ diff --git a/n/es-2006/src/sniff_rvb.c b/n/es-2006/src/sniff_rvb.c index e62ac17..8c5aef6 100644 --- a/n/es-2006/src/sniff_rvb.c +++ b/n/es-2006/src/sniff_rvb.c @@ -38,7 +38,7 @@ 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 = 4; +uint8_t sniff_rvb_ref_ratio = 3; /* test with new robot clothes */ /* ball black/white detection */ @@ -59,7 +59,7 @@ sniff_rvb_try_reference (uint8_t sensor_num, uint16_t { uint8_t compt; /* If sensor is valid */ - if (sensor_rvb_values[sensor_num][0] != 0) + if (sensor_rvb_values[sensor_num][0] < RVB_INVALID_CAPTURE) { /* For each color add this value by averrage */ for (compt = 0; compt < RVB_SNIFF_MAX_INDEX; compt++) @@ -86,7 +86,8 @@ sniff_rvb_ball (uint8_t sensor) 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; + else + return RVB_SNIFF_WHITE; } /* Test blue or red */ -- cgit v1.2.3