summaryrefslogtreecommitdiff
path: root/n/es-2007/src/sensor_rvb.c
diff options
context:
space:
mode:
Diffstat (limited to 'n/es-2007/src/sensor_rvb.c')
-rw-r--r--n/es-2007/src/sensor_rvb.c413
1 files changed, 413 insertions, 0 deletions
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;
+ }
+}
+