From 9a93020cb6c774e5280818648bb78b3978052e4f Mon Sep 17 00:00:00 2001 From: dufourj Date: Sat, 13 May 2006 22:36:09 +0000 Subject: --- n/es-2006/src/main.c | 11 ++- n/es-2006/src/sniff_rvb.c | 221 ++++++++++++++++++++++++++++++++++++++++++++++ n/es-2006/src/sniff_rvb.h | 69 +++++++++++++++ 3 files changed, 297 insertions(+), 4 deletions(-) create mode 100644 n/es-2006/src/sniff_rvb.c create mode 100644 n/es-2006/src/sniff_rvb.h (limited to 'n') 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 -- cgit v1.2.3