summaryrefslogtreecommitdiff
path: root/n/es-2006/src
diff options
context:
space:
mode:
Diffstat (limited to 'n/es-2006/src')
-rw-r--r--n/es-2006/src/main.c17
-rw-r--r--n/es-2006/src/sensor_rvb.c71
-rw-r--r--n/es-2006/src/sensor_rvb.h2
-rw-r--r--n/es-2006/src/sniff_rvb.c7
4 files changed, 66 insertions, 31 deletions
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 */