summaryrefslogtreecommitdiff
path: root/n/es-2006/src
diff options
context:
space:
mode:
authordufourj2006-05-13 22:36:09 +0000
committerdufourj2006-05-13 22:36:09 +0000
commit9a93020cb6c774e5280818648bb78b3978052e4f (patch)
tree829f2d42840ab83de04b298ad3d58403966e48eb /n/es-2006/src
parent28243d592481d45eaa8c077684c164bc4fb76c9b (diff)
Diffstat (limited to 'n/es-2006/src')
-rw-r--r--n/es-2006/src/main.c11
-rw-r--r--n/es-2006/src/sniff_rvb.c221
-rw-r--r--n/es-2006/src/sniff_rvb.h69
3 files changed, 297 insertions, 4 deletions
diff --git a/n/es-2006/src/main.c b/n/es-2006/src/main.c
index 078fc19..187232c 100644
--- a/n/es-2006/src/main.c
+++ b/n/es-2006/src/main.c
@@ -84,8 +84,11 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
v8_to_v16 (args[7], args[8]));
break;
/* Set current color as reference */
- case c ('r', 1):
- sniff_rvb_set_ref_color (args[0]);
+ case c ('r', 3):
+ temp_16b = v8_to_v16 (args[0], args[1]);
+ for (compt = 0; compt < RVB_MAX_SENSOR; compt++)
+ if (temp_16b & _BV(compt))
+ sniff_rvb_set_ref_color (compt, args[2]);
break;
/* Print stats if the sensor is seen */
case c ('A', 3):
@@ -139,8 +142,8 @@ main (void)
sei ();
// XXX
- sniff_rvb_set_threshold (RVB_SNIFF_RED, -100, -85, 725, 0);
- sniff_rvb_set_threshold (RVB_SNIFF_BLUE, 650, -140, -1050, -950);
+// sniff_rvb_set_threshold (RVB_SNIFF_RED, -100, -85, 725, 0);
+// sniff_rvb_set_threshold (RVB_SNIFF_BLUE, 650, -140, -1050, -950);
/* We are ready ! */
proto_send0 ('z');
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;
+ }
+}
diff --git a/n/es-2006/src/sniff_rvb.h b/n/es-2006/src/sniff_rvb.h
new file mode 100644
index 0000000..348a071
--- /dev/null
+++ b/n/es-2006/src/sniff_rvb.h
@@ -0,0 +1,69 @@
+#ifndef sniff_rvb_h
+#define sniff_rvb_h
+// sniff_rvb.h
+// 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 "io.h"
+#include "common.h"
+
+/** Analysis sensors and determine the colors. */
+
+#define RVB_SNIFF_MAX_INDEX 4
+#define RVB_SNIFF_GREEN 0
+#define RVB_SNIFF_BLUE 1
+#define RVB_SNIFF_RED 2
+#define RVB_SNIFF_OTHER 3
+
+/** Mode for color detection
+ * ONLY_GREEN is faster but tells only if it's green or not
+ **/
+#define RVB_SNIFF_ONLY_GREEN 0
+#define RVB_SNIFF_ALL_COLORS 1
+
+
+/** Set the current color seen by some sensors as the reference color.
+ * It must be called at the begining of the match for setting the green value.
+ */
+void sniff_rvb_set_ref_color (uint8_t sensor, uint8_t color);
+
+/** Set the threshold of the differents colors.
+ * There are used for the algorithm that distinguish 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);
+
+/** Analysis a color :
+ * - sensor : the sensor number to analysis ;
+ * - mode : for futur use.
+ * return the color number.
+ */
+uint8_t sniff_rvb_analysis_color (uint8_t sensor, uint8_t mode);
+
+/** Configure the sensor analysis system.
+ * - ref_ratio : ratio for the reference color.
+ */
+void sniff_rvb_config (uint8_t ref_ratio);
+
+#endif // sniff_rvb_h