/* 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; } }