summaryrefslogtreecommitdiff
path: root/n/es-2006/src/sensor_rvb.c
diff options
context:
space:
mode:
Diffstat (limited to 'n/es-2006/src/sensor_rvb.c')
-rw-r--r--n/es-2006/src/sensor_rvb.c274
1 files changed, 274 insertions, 0 deletions
diff --git a/n/es-2006/src/sensor_rvb.c b/n/es-2006/src/sensor_rvb.c
new file mode 100644
index 0000000..ac8b0aa
--- /dev/null
+++ b/n/es-2006/src/sensor_rvb.c
@@ -0,0 +1,274 @@
+/* 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/uart/uart.h"
+
+/**
+ * Somes defines.
+ **/
+/** Color pins. */
+#define S1RVB 7
+#define S0RVB 6
+/** Max input capture before considering we can start the real capture. */
+#define RVB_MAX_FALSE_IC 3
+/** 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 input capture */
+#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 */
+
+/** 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. */
+uint8_t sensor_rvb_overflow_count_;
+/** Results table for 9 RVB sensors (RVCB) */
+volatile uint16_t sensor_rvb_values[RVB_MAX_SENSOR][4];
+/** Geting stats only for one sensors and disabling computing the others. */
+uint8_t sensor_rvb_enable;
+/** Count the number of IC before starting a good capture. */
+uint8_t sensor_rvb_ic_count_;
+
+/** Select a color :
+ * 0 : red ;
+ * 1 : blue ;
+ * 2 : clear ;
+ * 3 : green. */
+inline uint8_t
+sensor_rvb_color_select (uint8_t sensor_rvb_color)
+{
+ /* Check */
+ if (sensor_rvb_color > 3)
+ return 0;
+ /* Select the color */
+ PORTF = (PORTF & ~0xC0) | (sensor_rvb_color << 6);
+ /* Wait a little */
+ utils_delay_ns (RVB_SENSOR_DELAY_IO);
+ return 1;
+}
+
+/** Enable a RVB sensor. */
+inline uint8_t
+sensor_rvb_sensor_select (uint8_t sensor_rvb_num)
+{
+ switch (sensor_rvb_num)
+ {
+ case 1:
+ case 2:
+ case 3:
+ case 4:
+ case 5:
+ case 6:
+ case 7:
+ PORTC = ~_BV(sensor_rvb_num);
+ 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;
+}
+
+/** Update everything for you. */
+void sensor_rvb_update (void);
+
+/** Initialisation of the RVB sensors module. */
+void
+sensor_rvb_init (void)
+{
+ /* We enable the system */
+ sensor_rvb_enable = 1;
+ /* 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);
+ /* Initialisation of counter :
+ * Noise canceler, 1 for prescaling */
+ TCCR1B |= _BV(ICNC1) | _BV(CS10);
+ /* XXX Fast PWM 10-bit */
+ TCCR1B |= _BV(WGM12);
+ TCCR1A |= _BV(WGM11) | _BV(WGM10);
+ /* XXX Overflow IR */
+ TIMSK |= _BV (TOIE1);
+ /* Nothing to do for the moment */
+ sensor_rvb_state_ = RVB_STATE_SLEEP;
+}
+
+/** Start updating RVBC values for sensors. */
+void
+sensor_rvb_start_capture (void)
+{
+ /* Are we already getting a sensor ? */
+ if (!sensor_rvb_state_ && sensor_rvb_enable)
+ {
+ sensor_rvb_state_ = RVB_STATE_NEXT_SENSOR;
+ /* Start with sensor one */
+ sensor_rvb_number_ = 0;
+ /* Select it and launch capture */
+ sensor_rvb_update ();
+ }
+}
+
+/** Watch only one sensor. Usefull for debugging. *//*{{{*/
+void
+sensor_rvb_watch (uint8_t sensor_num, uint8_t sensor_col)
+{
+ // XXX Better approach.
+ /* Disable all interrupt that could be enabled */
+ TIMSK &= ~_BV (TICIE1);
+ TIMSK &= ~_BV (TOIE1);
+ /* Put ourself in off mode */
+ sensor_rvb_enable = 0;
+ sensor_rvb_state_ = RVB_STATE_WATCH;
+ /* Select the color */
+ sensor_rvb_color_select (sensor_rvb_color_ = sensor_col);
+ /* Disable sensors */
+ sensor_rvb_sensor_select (0);
+ /* Wait a little */
+ utils_delay_ns (RVB_SENSOR_DELAY_IO);
+ /* Select sensor here */
+ sensor_rvb_sensor_select (sensor_num + 1);
+ /* Wait for output of sensor to be ready */
+ utils_delay_ns (RVB_SENSOR_DELAY_IO);
+}
+/*}}}*/
+
+/** 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;
+ /* Enable interrupt for IC1 and counter overflow */
+ 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 IC interrupt */
+ TIMSK &= ~_BV (TICIE1);
+ 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;
+ /* Enable interrupt for IC1 and counter overflow */
+ TIMSK |= _BV (TICIE1);
+ }
+}
+
+/** Timer 1 overflow. */
+SIGNAL (SIG_OVERFLOW1)
+{
+ if ((sensor_rvb_state_ == RVB_STATE_STOP_CAPTURE) &&
+ (++sensor_rvb_overflow_count_ == 4))
+ {
+ /* Disable IC interrupt */
+ TIMSK &= ~_BV (TICIE1);
+ /* Ask for next sensor */
+ sensor_rvb_state_ = RVB_STATE_NEXT_SENSOR;
+ sensor_rvb_update ();
+ }
+}
+
+/** Interrupt on falling edge for Input Capture 1. */
+SIGNAL (SIG_INPUT_CAPTURE1)
+{
+ switch (sensor_rvb_state_)
+ {
+ case RVB_STATE_WAIT_IC:
+ sensor_rvb_ic_count_ = RVB_MAX_FALSE_IC;
+ sensor_rvb_state_ = RVB_STATE_WAIT_IC2;
+ 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:
+ /* Save capture start */
+ sensor_rvb_value_ = ICR1;
+ /* New capture */
+ sensor_rvb_overflow_count_ = 0;
+ sensor_rvb_state_ = RVB_STATE_STOP_CAPTURE;
+ break;
+ case RVB_STATE_STOP_CAPTURE:
+ /* Disable IC interrupt */
+ TIMSK &= ~_BV (TICIE1);
+ /* Compute value */
+ sensor_rvb_value_ = ICR1 + sensor_rvb_overflow_count_ * TC1_TOP -
+ sensor_rvb_value_;
+ /* Save */
+ sensor_rvb_values[sensor_rvb_number_ - 1][sensor_rvb_color_] = sensor_rvb_value_;
+ sensor_rvb_state_ = RVB_STATE_NEXT_COLOR;
+ /* Next please */
+ sensor_rvb_update ();
+ break;
+ }
+}
+