/* 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 "string.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]; /** 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; /* test with new robot clothes */ /* ball black/white detection */ uint16_t green_limit = 7000; uint16_t clear_limit = 8000; /** 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] < RVB_INVALID_CAPTURE) { /* 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 "barillet" black/white */ uint8_t sniff_rvb_ball (uint8_t sensor) { if ((sensor_rvb_values[sensor][RVB_INDEX_GREEN] > green_limit) && (sensor_rvb_values[sensor][RVB_INDEX_CLEAR] > clear_limit)) return RVB_SNIFF_BLACK; else return RVB_SNIFF_WHITE; } /* Test blue or red */ uint8_t sniff_rvb_analysis_color_other (uint8_t sensor, uint8_t color) { uint8_t what_is_it = RVB_SNIFF_OTHER; if (color == RVB_SNIFF_BLUE) { if (sensor_rvb_values[sensor][RVB_INDEX_GREEN] > sniff_rvb_reference_color[sensor][RVB_INDEX_GREEN]) what_is_it = RVB_SNIFF_BLUE; } else if (color == RVB_SNIFF_RED) { if ((sensor_rvb_values[sensor][RVB_INDEX_RED] < sniff_rvb_reference_color[sensor][RVB_INDEX_RED]) && (sensor_rvb_values[sensor][RVB_INDEX_CLEAR] > sniff_rvb_reference_color[sensor][RVB_INDEX_CLEAR]) && (sensor_rvb_values[sensor][RVB_INDEX_GREEN] > sniff_rvb_reference_color[sensor][RVB_INDEX_GREEN])) what_is_it = RVB_SNIFF_RED; } return what_is_it; } /** Analysis a color */ uint8_t sniff_rvb_analysis_color (uint8_t sensor, uint8_t mode) { 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)) return what_is_it; /* test for red */ if (sniff_rvb_analysis_color_other (sensor, RVB_SNIFF_RED)) { what_is_it = RVB_SNIFF_RED; return what_is_it; } /* test for blue */ if (sniff_rvb_analysis_color_other (sensor, RVB_SNIFF_BLUE)) { what_is_it = RVB_SNIFF_BLUE; return what_is_it; } /* else other */ what_is_it = RVB_SNIFF_OTHER; return what_is_it; } /* 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); } } /** 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: break; } }