/* sensor_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 "sensor_rvb.h" #include "io.h" #include "modules/utils/utils.h" /* regv */ #include "modules/proto/proto.h" #include "sniff_rvb.h" #include "timer_1.h" /** * Somes defines. **/ /** Colors selection pins. */ #define S1RVB 7 #define S0RVB 6 /** Max input capture before considering we can start the real capture. * (default value) */ #define RVB_MAX_FALSE_IC 3 /** Max overflow of the timer 1 before thinking the sensor is HS. */ /* TODO Find a way to compute this value */ #define RVB_MAX_OVERFLOW 253 /** Wait time between IO change in ns. */ #define RVB_SENSOR_DELAY_IO 125 /** All the possible states. */ #define RVB_STATE_SLEEP 0 /* Doing nothing */ #define RVB_STATE_NEXT_SENSOR 1 /* Selecting a sensor */ #define RVB_STATE_WAIT_IC 20 /* Waiting for the first input capture */ #define RVB_STATE_WAIT_IC2 (RVB_STATE_WAIT_IC + 1) /* 2nd and more false input captures */ #define RVB_STATE_START_CAPTURE 2 /* After waiting enough IC, starting capture */ #define RVB_STATE_STOP_CAPTURE 3 /* We can compute a value */ #define RVB_STATE_NEXT_COLOR 4 /* Selecting another color */ #define RVB_STATE_WATCH 5 /* For debuging */ /*** Config ***/ /** Max input capture before considering we can start the real capture. */ // FIXME volatile uint8_t sensor_rvb_conf_max_false_ic_; /** Max overflow of the timer 1 before thinking the sensor is HS. */ uint8_t sensor_rvb_conf_max_ov_; /** RVB sensor state flag. */ volatile uint8_t sensor_rvb_state_; /** Internal computed result for one color. */ volatile uint16_t sensor_rvb_value_; /** Sensor used actually for computing value. */ volatile uint8_t sensor_rvb_number_; /** Color of the actual sensor. */ volatile uint8_t sensor_rvb_color_; /** Number of overflow interruption since last edge change. */ volatile uint8_t sensor_rvb_overflow_count_; /** Results table for 9 RVB sensors (RVCB) */ volatile uint16_t sensor_rvb_values[RVB_MAX_SENSOR][RVB_MAX_INDEX]; /** Count the number of IC before starting a good capture. */ uint8_t sensor_rvb_ic_count_; /** Barillet sensors are enabled ? */ volatile uint8_t sensor_rvb_barillet_flag_; volatile uint8_t sensor_rvb_low_sensor_flag_; /** We are not pooling all the sensors */ volatile uint8_t sensor_rvb_cur_; /** Update everything for you. */ inline void sensor_rvb_update (void); /** Are you sleeping */ uint8_t sensor_rvb_state_sleeeping (void) { return (sensor_rvb_state_ == RVB_STATE_SLEEP ? 1 : 0); } /** Select a color : * 0 : red ; * 1 : blue ; * 2 : clear ; * 3 : green. */ inline uint8_t sensor_rvb_color_select (uint8_t sensor_rvb_color) { switch (sensor_rvb_color) { case RVB_INDEX_RED: case RVB_INDEX_BLUE: case RVB_INDEX_CLEAR: case RVB_INDEX_GREEN: /* Select the color */ PORTF = (PORTF & ~0xC0) | (sensor_rvb_color << 6); /* Wait a little */ utils_delay_ns (RVB_SENSOR_DELAY_IO); /* Color exists */ return 1; default: /* Error */ return 0; } } /** Enable a RVB sensor. */ uint8_t sensor_rvb_sensor_select (uint8_t sensor_rvb_num) { switch (sensor_rvb_num) { case 1: case 2: case 3: case 4: PORTC = ~_BV(sensor_rvb_num); break; case 5: /* If we also want the other sensors */ if (sensor_rvb_low_sensor_flag_) { /* Normal behaviour */ PORTC = ~_BV(5); } else { /* Alternatively one sensor of the following */ if (sensor_rvb_cur_ == 0) PORTC = ~_BV(5); else PORTC = 0x7F; if (++sensor_rvb_cur_ == 2) sensor_rvb_cur_ = 0; } break; case 6: if (!sensor_rvb_low_sensor_flag_) { /* High for disable */ PORTC = 0xff; PORTD |= _BV(6); /* Wait a little */ utils_delay_ns (RVB_SENSOR_DELAY_IO); return 0; } else PORTC = ~_BV(6); break; case 7: PORTC = 0x7F; break; case 8: PORTD &= ~_BV(6); break; case 9: PORTC = ~_BV(0); break; default: /* High for disable */ PORTC = 0xff; PORTD |= _BV(6); /* Wait a little */ utils_delay_ns (RVB_SENSOR_DELAY_IO); return 0; } /* Wait a little */ utils_delay_ns (RVB_SENSOR_DELAY_IO); return 1; } /** Initialisation of the RVB sensors module. */ void sensor_rvb_init (void) { /* Init config */ sensor_rvb_conf_max_false_ic_ = RVB_MAX_FALSE_IC; sensor_rvb_conf_max_ov_ = RVB_MAX_OVERFLOW; /* No sensor selected */ sensor_rvb_sensor_select (0); /* Put ENA* pins in output */ DDRC = 0xff; DDRD |= _BV(6); /* Put color selector pins in output */ DDRF |= _BV(S0RVB) | _BV(S1RVB); /* Put asserv color states pins in output */ DDRA |= _BV(7) | _BV(6) | _BV(5) | _BV(4); /* Nothing to do for the moment */ sensor_rvb_state_ = RVB_STATE_SLEEP; } /** Configure some internal variables. */ void sensor_rvb_config (uint8_t false_ic, uint8_t max_ov) { sensor_rvb_conf_max_false_ic_ = false_ic; sensor_rvb_conf_max_ov_ = max_ov; } /** Start updating RVBC values for sensors. */ void sensor_rvb_start_capture (void) { /* Are we already getting a sensor ? */ if (sensor_rvb_state_ == RVB_STATE_SLEEP) { sensor_rvb_state_ = RVB_STATE_NEXT_SENSOR; /* Start with sensor one */ sensor_rvb_number_ = 0; /* Select it and launch capture */ sensor_rvb_update (); /* Ensure we have no pending interrupt for IC1. */ TIFR = _BV (ICF1); TIFR = _BV (TOV1); /* Enable interrupt for IC1 and counter overflow */ TIMSK |= _BV (TICIE1); TIMSK |= _BV (TOIE1); } } /** Update everything for you. */ void sensor_rvb_update (void) { if (sensor_rvb_state_ == RVB_STATE_NEXT_COLOR) { /* Select the color */ if (!sensor_rvb_color_select (++sensor_rvb_color_)) { /* No more colors for this sensor, next sensor please */ sensor_rvb_state_ = RVB_STATE_NEXT_SENSOR; } else { sensor_rvb_state_ = RVB_STATE_WAIT_IC; /* Ensure we have no pending interrupt for IC1. */ TIFR = _BV (ICF1); /* Enable interrupt for IC1 */ TIMSK |= _BV (TICIE1); } } if (sensor_rvb_state_ == RVB_STATE_NEXT_SENSOR) { /* Disable sensors */ sensor_rvb_sensor_select (0); /* Select sensor */ if (!sensor_rvb_sensor_select (++sensor_rvb_number_)) { /* Disable IC1 and TC1 overflow interrupts */ TIMSK &= ~_BV (TICIE1); TIMSK &= ~_BV (TOIE1); /* Finish ! Go to sleep */ sensor_rvb_state_ = RVB_STATE_SLEEP; return; } /* Start with first color */ sensor_rvb_color_select (sensor_rvb_color_ = 0); sensor_rvb_state_ = RVB_STATE_WAIT_IC; sensor_rvb_overflow_count_ = 0; /* Ensure we have no pending interrupt for IC1. */ TIFR = _BV (ICF1); /* Enable interrupt for IC1 */ TIMSK |= _BV (TICIE1); } } /** Timer 1 overflow. */ SIGNAL (SIG_OVERFLOW1) { switch (sensor_rvb_state_) { case RVB_STATE_STOP_CAPTURE: ++sensor_rvb_overflow_count_; break; case RVB_STATE_WAIT_IC: case RVB_STATE_WAIT_IC2: // XXX >= Sucks ! // TODO : check this >= and the IC2 used if (++sensor_rvb_overflow_count_ >= sensor_rvb_conf_max_ov_) { /* Invalidate the sensor */ if (!sensor_rvb_low_sensor_flag_ && (sensor_rvb_number_ == 5)) if (sensor_rvb_cur_ == 0) sensor_rvb_values[6][0] = RVB_INVALID_CAPTURE; else sensor_rvb_values[4][0] = RVB_INVALID_CAPTURE; else sensor_rvb_values[sensor_rvb_number_ - 1][0] = RVB_INVALID_CAPTURE; /* Disable IC interrupt */ TIMSK &= ~_BV (TICIE1); /* Ask for next sensor */ sensor_rvb_state_ = RVB_STATE_NEXT_SENSOR; sensor_rvb_update (); } break; } } /** Interrupt on falling edge for Input Capture 1. */ SIGNAL (SIG_INPUT_CAPTURE1) { /* Save it earlier ! */ uint16_t ic1_cache = ICR1; uint16_t result; switch (sensor_rvb_state_) { case RVB_STATE_WAIT_IC: sensor_rvb_ic_count_ = sensor_rvb_conf_max_false_ic_; sensor_rvb_state_ = RVB_STATE_WAIT_IC2; /* nobreak */ case RVB_STATE_WAIT_IC2: if (!--sensor_rvb_ic_count_) /* Last ignored capture */ sensor_rvb_state_ = RVB_STATE_START_CAPTURE; break; case RVB_STATE_START_CAPTURE: /* Manage case when overflow occured before IC but we are called first * */ if (!(ic1_cache & (TC1_TOP & ~(TC1_TOP >> 1)))) TIFR = _BV (TOV1); /* Save capture start */ sensor_rvb_value_ = ic1_cache; /* Reset overflow_count */ sensor_rvb_overflow_count_ = 0; /* New capture */ sensor_rvb_state_ = RVB_STATE_STOP_CAPTURE; break; case RVB_STATE_STOP_CAPTURE: /* Compute value */ result = ic1_cache + sensor_rvb_overflow_count_ * (TC1_TOP + 1) - sensor_rvb_value_; if (!sensor_rvb_low_sensor_flag_ && (sensor_rvb_number_ == 5)) if (sensor_rvb_cur_ == 0) sensor_rvb_values[6][sensor_rvb_color_] = result; else sensor_rvb_values[4][sensor_rvb_color_] = result; else sensor_rvb_values[sensor_rvb_number_ - 1][sensor_rvb_color_] = result; /* Disable IC interrupt */ TIMSK &= ~_BV (TICIE1); sensor_rvb_state_ = RVB_STATE_NEXT_COLOR; /* Next please */ sensor_rvb_update (); break; } } /** Update pins connected to the asserv AVR with the colors seen by the * sensors. */ void sensor_rvb_update_asserv_pins (void) { uint8_t compt; if (sensor_rvb_state_ == RVB_STATE_SLEEP) { for (compt = 0; compt < 4; compt++) { /* Is sensor capture valid */ if (sensor_rvb_values[compt][0] < RVB_INVALID_CAPTURE) { if (sniff_rvb_analysis_color (compt, RVB_SNIFF_ONLY_GREEN) != RVB_SNIFF_GREEN) PORTA |= _BV (compt + 4); else PORTA &= ~_BV (compt + 4); } } } // TODO: // - called the desired sensors with sensor_rvb_analysis_color ; // - choose with Ni ; // - do we need to update all the pins ? // - this functions have to be hightly configurable : for example, choose // if we can select frequency of update ; // - manage a case for knowing the mode of the robot in order to select // which pins we should update. } /** Enable/disable capture for black & white sensors (the ones inside the * barillet). */ void sensor_rvb_upper_sensors (uint8_t enable) { if (enable) sensor_rvb_low_sensor_flag_ = 1; else { /* Invalidate all capture */ // XXX Good idea ? sensor_rvb_low_sensor_flag_ = 0; sensor_rvb_values[8][0] = sensor_rvb_values[7][0] = RVB_INVALID_CAPTURE; } }