From 06ecd390219972734cb70c6ceb5aa74cadb0a042 Mon Sep 17 00:00:00 2001 From: leblanc Date: Sat, 10 Mar 2007 17:42:07 +0000 Subject: Copie de es-2006 en es-2007. --- n/es-2007/src/sensor_rvb.c | 413 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 413 insertions(+) create mode 100644 n/es-2007/src/sensor_rvb.c (limited to 'n/es-2007/src/sensor_rvb.c') diff --git a/n/es-2007/src/sensor_rvb.c b/n/es-2007/src/sensor_rvb.c new file mode 100644 index 0000000..46b2598 --- /dev/null +++ b/n/es-2007/src/sensor_rvb.c @@ -0,0 +1,413 @@ +/* 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; + } +} + -- cgit v1.2.3