summaryrefslogtreecommitdiff
path: root/n/es-2006/src/sensor_rvb.c
diff options
context:
space:
mode:
Diffstat (limited to 'n/es-2006/src/sensor_rvb.c')
-rw-r--r--n/es-2006/src/sensor_rvb.c125
1 files changed, 37 insertions, 88 deletions
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.
+}
+