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.c86
1 files changed, 86 insertions, 0 deletions
diff --git a/n/es-2006/src/sensor_rvb.c b/n/es-2006/src/sensor_rvb.c
index e98d62f..a8ccf5a 100644
--- a/n/es-2006/src/sensor_rvb.c
+++ b/n/es-2006/src/sensor_rvb.c
@@ -27,6 +27,7 @@
#include "io.h"
#include "modules/utils/utils.h" /* regv */
+#include "modules/proto/proto.h"
/**
* Somes defines.
@@ -74,6 +75,10 @@ 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);
@@ -138,6 +143,83 @@ 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)
@@ -153,6 +235,8 @@ sensor_rvb_init (void)
DDRD |= _BV(6);
/* Put color selector pins in output */
DDRF |= _BV(S0RVB) | _BV(S1RVB);
+ /* Put asserv color states pins in output */
+ DDRA |= _BV(7) | _BV(6) | _BV(5) | _BV(4);
/* Nothing to do for the moment */
sensor_rvb_state_ = RVB_STATE_SLEEP;
}
@@ -246,6 +330,8 @@ SIGNAL (SIG_OVERFLOW1)
// TODO : check this >= and the IC2 used
if (++sensor_rvb_overflow_count_ >= sensor_rvb_conf_max_ov_)
{
+ /* Invalidate the sensor */
+ sensor_rvb_values[sensor_rvb_number_ - 1][0] = 0;
/* Disable IC interrupt */
TIMSK &= ~_BV (TICIE1);
/* Ask for next sensor */