summaryrefslogtreecommitdiff
path: root/n/es-2006/src/sniff_rvb.c
diff options
context:
space:
mode:
Diffstat (limited to 'n/es-2006/src/sniff_rvb.c')
-rw-r--r--n/es-2006/src/sniff_rvb.c221
1 files changed, 221 insertions, 0 deletions
diff --git a/n/es-2006/src/sniff_rvb.c b/n/es-2006/src/sniff_rvb.c
new file mode 100644
index 0000000..c12b1bd
--- /dev/null
+++ b/n/es-2006/src/sniff_rvb.c
@@ -0,0 +1,221 @@
+/* sniff_rvb.c */
+/* es - Input/Output general purpose board. {{{
+ *
+ * Copyright (C) 2006 Dufour Jérémy
+ *
+ * Robot APB Team/Efrei 2004.
+ * Web: http://assos.efrei.fr/robot/
+ * Email: robot AT efrei DOT fr
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * }}} */
+#include "sniff_rvb.h"
+
+#include "io.h"
+#include "common.h"
+#include "modules/proto/proto.h"
+#include "modules/utils/byte.h"
+
+#include "sensor_rvb.h"
+
+/** Reference color. */
+uint16_t sniff_rvb_reference_color[RVB_MAX_SENSOR][RVB_SNIFF_MAX_INDEX];
+/** Threshold table data. */
+int16_t sniff_rvb_threshold_table[RVB_MAX_SENSOR][RVB_SNIFF_MAX_INDEX];
+/** Ratio min max */
+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;
+
+/** Configure the sensor analysis system. */
+void
+sniff_rvb_config (uint8_t ref_ratio)
+{
+ sniff_rvb_ref_ratio = ref_ratio;
+}
+/** Try to reference the sensor_num sensor into the reference color. */
+inline uint8_t
+sniff_rvb_try_reference (uint8_t sensor_num, uint16_t
+ ref[RVB_MAX_SENSOR][RVB_SNIFF_MAX_INDEX])
+{
+ uint8_t compt;
+
+ /* If sensor is valid */
+ if (sensor_rvb_values[sensor_num][0] != 0)
+ {
+ /* For each color add this value by averrage */
+ for (compt = 0; compt < RVB_SNIFF_MAX_INDEX; compt++)
+ {
+ if (ref[sensor_num][compt] == 0)
+ ref[sensor_num][compt] =
+ sensor_rvb_values[sensor_num][compt];
+ else
+ ref[sensor_num][compt] =
+ (sensor_rvb_values[sensor_num][compt] / 2
+ + ref[sensor_num][compt] / 2);
+ }
+ return !0;
+ }
+ return 0;
+}
+
+
+/* Test blue or red */
+uint8_t
+sniff_rvb_analysis_color_other (uint8_t sensor, uint8_t color)
+{
+ uint8_t compt;
+ uint8_t what_is_it = color;
+ for (compt = 0; compt < 4; compt++)
+ {
+ // XXX will be in cache table after
+ uint16_t threshold = sniff_rvb_reference_color[sensor][compt]
+ + sniff_rvb_threshold_table[color][compt];
+ // if relative threshold is positive, current value must be greater
+ // than the threshold
+ if (sniff_rvb_threshold_table[color][compt] > 0)
+ {
+ if (sensor_rvb_values[sensor][compt] > threshold)
+ {
+ continue;
+ }
+ }
+ // if negative, must be lower
+ else
+ {
+ if (sensor_rvb_values[sensor][compt] < threshold)
+ {
+ continue;
+ }
+ }
+ // if conditions not true, color is not blue
+ what_is_it = RVB_SNIFF_OTHER;
+ break;
+ }
+ return what_is_it;
+}
+
+/** Analysis a color */
+uint8_t
+sniff_rvb_analysis_color (uint8_t sensor, uint8_t mode)
+{
+ static uint8_t hysteresis[RVB_MAX_SENSOR] = {RVB_SNIFF_GREEN};
+ uint8_t compt, what_is_it = RVB_SNIFF_GREEN;
+ /* test for green */
+ for (compt = 0; compt < RVB_SNIFF_MAX_INDEX; compt++)
+ {
+ if ((sensor_rvb_values[sensor][compt] < sniff_rvb_reference_color_min[sensor][compt])
+ || (sensor_rvb_values[sensor][compt] > sniff_rvb_reference_color_max[sensor][compt]))
+ {
+ what_is_it = RVB_SNIFF_OTHER;
+ break;
+ }
+ }
+
+ if ((what_is_it == RVB_SNIFF_GREEN) || (mode == RVB_SNIFF_ONLY_GREEN))
+ goto sniff_rvb_set_ref_color_end;
+
+// XXX pb if reference too high and sum with threshold overflow
+// same pb with the cache table creation
+// XXX create cache table for blue and red
+
+ /* test for blue */
+ if (sniff_rvb_analysis_color_other (sensor, RVB_SNIFF_BLUE) == RVB_SNIFF_BLUE)
+ {
+ what_is_it = RVB_SNIFF_BLUE;
+ goto sniff_rvb_set_ref_color_end;
+ }
+ /* test for red */
+ if (sniff_rvb_analysis_color_other (sensor, RVB_SNIFF_RED) == RVB_SNIFF_RED)
+ {
+ what_is_it = RVB_SNIFF_RED;
+ goto sniff_rvb_set_ref_color_end;
+ }
+ /* else other */
+ what_is_it = RVB_SNIFF_OTHER;
+
+/* sometimes it's goooood ! */
+sniff_rvb_set_ref_color_end:
+ if (what_is_it == hysteresis[sensor])
+ return what_is_it;
+ else
+ {
+ hysteresis[sensor] = what_is_it;
+ return RVB_SNIFF_GREEN;
+ }
+}
+
+
+/* sub function of sniff_rvb_set_ref_color */
+void
+sniff_rvb_set_ref_color_green (uint8_t sensor)
+{
+ uint8_t compt;
+ if (sniff_rvb_try_reference (sensor, sniff_rvb_reference_color))
+ for (compt = 0; compt < RVB_SNIFF_MAX_INDEX; compt++)
+ {
+ 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_max[sensor][compt] =
+ sniff_rvb_reference_color[sensor][compt] +
+ (sniff_rvb_reference_color[sensor][compt] / sniff_rvb_ref_ratio);
+ }
+}
+
+/* sub function of sniff_rvb_set_ref_color */
+void
+sniff_rvb_set_ref_color_other (uint8_t sensor)
+{
+
+}
+
+/** Set the current color seen by some sensors as the reference color
+ * and do some cache creation for preventing useless computing. */
+void
+sniff_rvb_set_ref_color (uint8_t sensor, uint8_t color)
+{
+ switch (color)
+ {
+ case RVB_SNIFF_GREEN:
+ sniff_rvb_set_ref_color_green (sensor);
+ break;
+ case RVB_SNIFF_RED:
+ case RVB_SNIFF_BLUE:
+ sniff_rvb_set_ref_color_other (sensor);
+ break;
+ }
+}
+
+/** Set the threshold of the differents colors. */
+void
+sniff_rvb_set_threshold (uint8_t color, int16_t red_ts, int16_t blue_ts,
+ int16_t clear_ts, int16_t green_ts)
+{
+ // XXX no green needed
+ switch (color)
+ {
+ case RVB_SNIFF_RED:
+ case RVB_SNIFF_BLUE:
+ case RVB_SNIFF_GREEN:
+ sniff_rvb_threshold_table[color][0] = red_ts;
+ sniff_rvb_threshold_table[color][1] = blue_ts;
+ sniff_rvb_threshold_table[color][2] = clear_ts;
+ sniff_rvb_threshold_table[color][3] = green_ts;
+ break;
+ }
+}