summaryrefslogtreecommitdiff
path: root/n/es-2006/src
diff options
context:
space:
mode:
authordufourj2006-05-23 08:47:34 +0000
committerdufourj2006-05-23 08:47:34 +0000
commit4d1bbfbad04a9af0436fc1709ac301f19fea9260 (patch)
treee9947dc2abca8c8b761c0931810b86bf8cb10ef4 /n/es-2006/src
parent7d808f18089edd6763a630d5bc40695698c81566 (diff)
ES :
* LCD : - création de fonctions et découpage dans un autre fichier ; - interfacage fonctionnel. TODO : gestion de nombres de caractères différent de 32 + grub. * Barrillet : - correction d'un warning de link. * RVB : - correction d'un bug en mode ou on ne regarde pas tous les capteurs ; - meilleur détection du bleue. * Servo moteur : - bonnes valeurs et fonction de gestion du servo poubelle. * Sharp : - fonctions de lecture des sharps, sans seuil pour le moment.
Diffstat (limited to 'n/es-2006/src')
-rw-r--r--n/es-2006/src/Makefile5
-rw-r--r--n/es-2006/src/avrconfig.h8
-rw-r--r--n/es-2006/src/barillet.h3
-rw-r--r--n/es-2006/src/lcd.c56
-rw-r--r--n/es-2006/src/lcd.h45
-rw-r--r--n/es-2006/src/main.c128
-rw-r--r--n/es-2006/src/others.h40
-rw-r--r--n/es-2006/src/sensor_rvb.c33
-rw-r--r--n/es-2006/src/servo_motor.c59
-rw-r--r--n/es-2006/src/servo_motor.h9
-rw-r--r--n/es-2006/src/sharp.c59
-rw-r--r--n/es-2006/src/sharp.h54
-rw-r--r--n/es-2006/src/sniff_rvb.c31
13 files changed, 447 insertions, 83 deletions
diff --git a/n/es-2006/src/Makefile b/n/es-2006/src/Makefile
index 8abab90..9ae231c 100644
--- a/n/es-2006/src/Makefile
+++ b/n/es-2006/src/Makefile
@@ -1,7 +1,8 @@
BASE = ../../avr
PROGS = es
-es_SOURCES = main.c sensor_rvb.c sniff_rvb.c timer_1.c barillet.c servo_motor.c
-MODULES = proto uart utils
+es_SOURCES = main.c sensor_rvb.c sniff_rvb.c timer_1.c barillet.c \
+ servo_motor.c sharp.c lcd.c
+MODULES = proto uart utils adc twi
CONFIGFILE = avrconfig.h
# atmega8, atmega8535, atmega128...
AVR_MCU = atmega64
diff --git a/n/es-2006/src/avrconfig.h b/n/es-2006/src/avrconfig.h
index 2c2c103..eced8a7 100644
--- a/n/es-2006/src/avrconfig.h
+++ b/n/es-2006/src/avrconfig.h
@@ -25,6 +25,12 @@
*
* }}} */
+/* I'am a twi master ! */
+#define TWI_SL_RCPT_SIZE 1
+#define TWI_SL_SEND_SIZE 1
+
+#define TWI_MASTER_ENABLE 1
+
/* global */
/** AVR Frequency : 1000000, 1843200, 2000000, 3686400, 4000000, 7372800,
* 8000000, 11059200, 14745600, 16000000, 18432000, 20000000. */
@@ -75,7 +81,7 @@
/* proto - Protocol module. */
/** Maximum argument size. */
-#define AC_PROTO_ARGS_MAX_SIZE 8
+#define AC_PROTO_ARGS_MAX_SIZE 32
/** Callback function name. */
#define AC_PROTO_CALLBACK proto_callback
/** Putchar function name. */
diff --git a/n/es-2006/src/barillet.h b/n/es-2006/src/barillet.h
index b783782..13d1af0 100644
--- a/n/es-2006/src/barillet.h
+++ b/n/es-2006/src/barillet.h
@@ -55,5 +55,8 @@ void extraction(void);
/* fonction pour setter un depot de balle */
void depot_balle(void);
+/* Dodo. */
+void barillet_sleep (void);
+
#endif
diff --git a/n/es-2006/src/lcd.c b/n/es-2006/src/lcd.c
new file mode 100644
index 0000000..e7cff75
--- /dev/null
+++ b/n/es-2006/src/lcd.c
@@ -0,0 +1,56 @@
+/* lcd.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 "lcd.h"
+
+#include "modules/twi/twi.h"
+
+uint8_t grub_greeting[32] = "Booting : press 1-NFS 2-Auto 3-N";
+
+/* Init communication with the LCD. */
+void lcd_init (void)
+{
+ /* Init twi */
+ twi_init (TWI_LCD_ADDR);
+}
+
+/* Print something on the LCD */
+inline void lcd_print (uint8_t *string, uint8_t size)
+{
+ /* Send to the LCD */
+ twi_ms_send (TWI_LCD_ADDR, string, size);
+}
+
+/* What key is pressed into the LCD keyboard ? */
+uint8_t lcd_key (void)
+{
+ static uint8_t buffer_read;
+ twi_ms_read (TWI_LCD_ADDR, &buffer_read, 1);
+ return buffer_read;
+}
+
+void lcd_grub_booting (void)
+{
+ lcd_print (grub_greeting, 32);
+}
diff --git a/n/es-2006/src/lcd.h b/n/es-2006/src/lcd.h
new file mode 100644
index 0000000..ebd93a0
--- /dev/null
+++ b/n/es-2006/src/lcd.h
@@ -0,0 +1,45 @@
+#ifndef lcd_h
+#define lcd_h
+// lcd.h
+// 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.
+//
+// }}}
+/** Communication with the LCD card. */
+
+#include "common.h"
+
+#define TWI_LCD_ADDR 0x02
+
+/* Init communication with the LCD. */
+void lcd_init (void);
+
+/* Print something on the LCD */
+inline void lcd_print (uint8_t *string, uint8_t size);
+
+/* What key is pressed into the LCD keyboard ? */
+uint8_t lcd_key (void);
+
+/* Tell the LCD we have seen the Grub ! */
+void lcd_grub_booting (void);
+
+#endif // lcd_h
diff --git a/n/es-2006/src/main.c b/n/es-2006/src/main.c
index ecc80f0..8320bfd 100644
--- a/n/es-2006/src/main.c
+++ b/n/es-2006/src/main.c
@@ -35,9 +35,10 @@
#include "sniff_rvb.h" /* RVB sensors analysis */
#include "timer_1.h" /* timer/counter 1 */
#include "others.h" /* define game color mode + jack + frontal sensor */
+#include "servo_motor.h"/* Servo motor module */
+#include "sharp.h" /* SHarps module */
+#include "lcd.h" /* Communication with the LCD card */
#include "barillet.h"
-#include "servo_motor.h"
-
/* Statistics for RVB sensors */
uint8_t sensor_rvb_stat_enable[RVB_MAX_SENSOR], sensor_rvb_stats[RVB_MAX_SENSOR];
@@ -47,6 +48,23 @@ uint8_t sniff_rvb_stat_enable[RVB_MAX_SENSOR], sniff_rvb_stats[RVB_MAX_SENSOR];
/* Statistics for RVB ball sensors. */
uint8_t sniff_ball_stat_enable[2], sniff_ball_stats[2];
+/* Config and statistics for SHarps */
+uint8_t sharp_stat_enable[SHARP_NUMBER], sharp_stat_freq[SHARP_NUMBER];
+uint8_t sharp_enable[SHARP_NUMBER], sharp_enable_freq[SHARP_NUMBER];
+
+/* Stats for colour selection and jack */
+uint8_t others_jackcolor_stat_enable, others_jackcolor_stats;
+
+/* XXX Debug stats */
+uint8_t main_stats[4];
+uint8_t main_stats_freq, main_stats_enable;
+
+/* Twi */
+uint8_t twi_buffer_read;
+
+/* Keyboard LCD update frequency */
+uint8_t lcd_key_enable, lcd_key_freq;
+
/** Call when we receive some data from proto (uart) */
void
proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
@@ -64,6 +82,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
utils_reset ();
break;
+ case c ('Z', 1):
+ main_stats_freq = main_stats_enable = args[0];
+ break;
+
/*** RVB ***/
/* RVB sensors configuration */
case c ('p', 3):
@@ -119,11 +141,42 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
case c ('k', 0 ):
proto_send1b ('k', others_selectcoul());
break;
+ /* Selection color & jack */
+ case c ('q', 1):
+ others_jackcolor_stat_enable = others_jackcolor_stats = args[0];
+ break;
+
/* Communication with the servo motor */
case c ('m', 2):
servo_motor_set_pos (args[0], args[1]);
break;
+ /* SHarps */
+ /* Update frequency */
+ case c ('h', 2):
+ for (compt = 0; compt < SHARP_NUMBER; compt++)
+ if (args[0] & _BV(compt))
+ sharp_enable[compt] = sharp_enable_freq[compt] = args[1];
+ break;
+ /* Stats frequency */
+ case c ('H', 2):
+ for (compt = 0; compt < SHARP_NUMBER; compt++)
+ if (args[0] & _BV(compt))
+ sharp_stat_enable[compt] = sharp_stat_freq[compt] = args[1];
+ break;
+
+ /* LCD */
+ case c ('L', 32):
+ lcd_print (args, 32);
+ break;
+ case c ('l', 1):
+ lcd_key_freq = lcd_key_enable = args[0];
+ break;
+
+ /* Grub ! */
+ case c ('G', 0):
+ lcd_grub_booting ();
+ break;
/* tests primaires pour le barillet */
case c('v',3):
@@ -175,7 +228,6 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
int
main (void)
{
- uint8_t init_rvb = 200;
uint8_t compt;
/* Serial port */
@@ -183,7 +235,7 @@ main (void)
/* Main timer init */
timer_main_init ();
/* Init timer/counter 1 for RVB & PWM
- * /!\ Must be called before RVB/PWM init ! */
+ * /!\ Must be called before RVB/PWM init /!\ */
timer1_init ();
/* Init RVB sensors system */
sensor_rvb_init ();
@@ -193,6 +245,11 @@ main (void)
barillet_init();
/* Servo motor init */
servo_motor_init ();
+ /* Sharp init */
+ sharp_init ();
+ /* LCD init */
+ lcd_init ();
+
/* Enable interrupts */
sei ();
@@ -206,8 +263,18 @@ main (void)
// possible.
timer_main_wait ();
+ if (main_stats_enable && !--main_stats_freq)
+ {
+ main_stats_freq = main_stats_enable;
+ proto_send4b ('Z', main_stats[0], main_stats[1], main_stats[2],
+ main_stats[3]);
+ }
+
+ main_stats[0] = TCNT0;
+
/* RVB Sensors stats */
for (compt = 0; compt < RVB_MAX_SENSOR; compt++)
+ {
if (sensor_rvb_stat_enable[compt] && !--sensor_rvb_stats[compt])
{
/* Re init stats system for this sensor */
@@ -218,15 +285,14 @@ main (void)
sensor_rvb_values[compt][2],
sensor_rvb_values[compt][3]);
}
-
- /* RVB Sensors analysis stats */
- for (compt = 0; compt < RVB_MAX_SENSOR; compt++)
+ /* RVB Sensors analysis stats */
if (sniff_rvb_stat_enable[compt] && !--sniff_rvb_stats[compt])
{
/* Re init stats system for this sensor */
sniff_rvb_stats[compt] = sniff_rvb_stat_enable[compt];
proto_send2b ('A', compt, sniff_rvb_analysis_color (compt, RVB_SNIFF_ALL_COLORS));
}
+ }
/* Print stats for ball */
for (compt = 0; compt < 2; compt++)
@@ -237,26 +303,49 @@ main (void)
proto_send2b ('B', compt, sniff_rvb_analysis_ball (compt + 7));
}
+ main_stats[1] = TCNT0;
+
/* Update all the asserv pin comunication */
sensor_rvb_update_asserv_pins ();
- // FIXME, if we can put some of this values into the eeprom, it will
- // be better...
- if (init_rvb > 0)
+ if (others_jackcolor_stat_enable && !--others_jackcolor_stats)
{
- init_rvb--;
- if ((init_rvb == 1) || (init_rvb == 2))
- {
- sniff_rvb_set_ref_color (0, 0);
- sniff_rvb_set_ref_color (1, 0);
- sniff_rvb_set_ref_color (2, 0);
- sniff_rvb_set_ref_color (3, 0);
- }
+ others_jackcolor_stats = others_jackcolor_stat_enable;
+ proto_send1b ('q', other_jack_color ());
+ }
+ /* LCD */
+ if (lcd_key_enable && !--lcd_key_freq)
+ {
+ lcd_key_freq = lcd_key_enable;
+ if ((twi_buffer_read = lcd_key ()))
+ proto_send1b ('l', twi_buffer_read);
}
/* gestion du barilet */
sequenceur_barillet ();
+ /* Sharps */
+ for (compt = 0; compt < SHARP_NUMBER; compt++)
+ {
+ /* Update */
+ if (sharp_enable[compt] && !--sharp_enable_freq[compt])
+ {
+ /* Re init update system for this sharp */
+ sharp_enable_freq[compt] = sharp_enable[compt];
+ sharp_update_values (compt);
+ }
+ /* Stats */
+ if (sharp_stat_enable[compt] && !--sharp_stat_freq[compt])
+ {
+ /* Re init stats system for this sharp */
+ sharp_stat_freq[compt] = sharp_stat_enable[compt];
+ /* XXX 16 bits sucks */
+ proto_send2w ('H', compt, sharp_values[compt]);
+ }
+ }
+
+ main_stats[2] = TCNT0;
+
/* Get data for serial port */
while (uart0_poll ())
proto_accept (uart0_getc ());
@@ -265,5 +354,8 @@ main (void)
* I want this to be done as later as possible, and espceically after
* commands has been parsed ! */
sensor_rvb_start_capture ();
+
+ main_stats[3] = TCNT0;
}
}
+
diff --git a/n/es-2006/src/others.h b/n/es-2006/src/others.h
index dd88722..0e25348 100644
--- a/n/es-2006/src/others.h
+++ b/n/es-2006/src/others.h
@@ -3,7 +3,7 @@
// others.h
// es - Input/Output general purpose board. {{{
//
-// Copyright (C) 2006 Dufour Jérémy
+// Copyright (C) 2006 Patrick Goncalves - (Dufour Jérémy)
//
// Robot APB Team/Efrei 2004.
// Web: http://assos.efrei.fr/robot/
@@ -40,31 +40,31 @@
/* access to value of pins of PORTA */
#define GET_PIN_OF_PORTA(n) ( PINA & _BV(n) )
-// /* SET DDRA */
-// /* set bit of DDR to 0 in order to set the pin n of PORTA in mode in */
-// #define SET_PIN_IN_MODE_IN_OF_PORTA(n) ( DDRA &= ~_BV(n) )
-// /* set bit of DDR to 1 in order to set the pin n of PORTA in mode out */
-// #define SET_PIN_IN_MODE_OUT_OF_PORTA(n) ( DDRA |= ~_BV(n) )
-
/** return the value of pin select color */
-inline uint8_t others_selectcoul(void)
+inline uint8_t others_selectcoul (void)
{
- return GET_PIN_OF_PORTA(SELECT_COLOR_PIN);
-};
+ return GET_PIN_OF_PORTA (SELECT_COLOR_PIN);
+}
/** return the value of pin jack */
-inline uint8_t others_jack(void)
+inline uint8_t others_jack (void)
{
- return GET_PIN_OF_PORTA(JACK_PIN);
-};
+ return GET_PIN_OF_PORTA (JACK_PIN);
+}
/** return the value of pin contact */
-inline uint8_t others_contact(void)
+inline uint8_t others_contact (void)
{
- return GET_PIN_OF_PORTA(CONTACT_PIN);
-};
+ return GET_PIN_OF_PORTA (CONTACT_PIN);
+}
+
+/** Return the jack and the select_colour at the same time */
+inline uint8_t other_jack_color (void)
+{
+ return (PINA & 0x03);
+}
-/** initialisation of color button (put select color pin in mode 'IN') */
+/** Initialisation of color button (put select color pin in mode 'IN') */
inline void others_init(void)
{
/* init DDRA (0 default) */
@@ -74,9 +74,7 @@ inline void others_init(void)
/* DDRA &= ~_BV(SELECT_COLOR_PIN); */
/* pull up in mode in */
- PORTA = PORTA | _BV(SELECT_COLOR_PIN) | _BV(JACK_PIN) | _BV(CONTACT_PIN);
-};
-
-
+ PORTA |= _BV(SELECT_COLOR_PIN) | _BV(JACK_PIN) | _BV(CONTACT_PIN);
+}
#endif // others_h
diff --git a/n/es-2006/src/sensor_rvb.c b/n/es-2006/src/sensor_rvb.c
index fae1c27..9d15223 100644
--- a/n/es-2006/src/sensor_rvb.c
+++ b/n/es-2006/src/sensor_rvb.c
@@ -23,13 +23,13 @@
*
* }}} */
#include "sensor_rvb.h"
-#include "timer_1.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.
@@ -75,12 +75,14 @@ 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][4];
+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);
@@ -115,7 +117,6 @@ sensor_rvb_color_select (uint8_t sensor_rvb_color)
uint8_t
sensor_rvb_sensor_select (uint8_t sensor_rvb_num)
{
- static uint8_t sensor_cur = 0;
switch (sensor_rvb_num)
{
case 1:
@@ -134,12 +135,12 @@ sensor_rvb_sensor_select (uint8_t sensor_rvb_num)
else
{
/* Alternatively one sensor of the following */
- if (sensor_cur == 0)
+ if (sensor_rvb_cur_ == 0)
PORTC = ~_BV(5);
else
PORTC = 0x7F;
- if (++sensor_cur == 2)
- sensor_cur = 0;
+ if (++sensor_rvb_cur_ == 2)
+ sensor_rvb_cur_ = 0;
}
break;
case 6:
@@ -287,7 +288,13 @@ SIGNAL (SIG_OVERFLOW1)
if (++sensor_rvb_overflow_count_ >= sensor_rvb_conf_max_ov_)
{
/* Invalidate the sensor */
- sensor_rvb_values[sensor_rvb_number_ - 1][0] = RVB_INVALID_CAPTURE;
+ 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 */
@@ -303,6 +310,7 @@ SIGNAL (SIG_INPUT_CAPTURE1)
{
/* Save it earlier ! */
uint16_t ic1_cache = ICR1;
+ uint16_t result;
switch (sensor_rvb_state_)
{
case RVB_STATE_WAIT_IC:
@@ -328,9 +336,16 @@ SIGNAL (SIG_INPUT_CAPTURE1)
break;
case RVB_STATE_STOP_CAPTURE:
/* Compute value */
- sensor_rvb_values[sensor_rvb_number_ - 1][sensor_rvb_color_] =
- ic1_cache + sensor_rvb_overflow_count_ * (TC1_TOP + 1) -
+ 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;
diff --git a/n/es-2006/src/servo_motor.c b/n/es-2006/src/servo_motor.c
index d01ceac..cd919c7 100644
--- a/n/es-2006/src/servo_motor.c
+++ b/n/es-2006/src/servo_motor.c
@@ -41,17 +41,16 @@
#define SRVM_TOTEML_PIN 3 /* first totem */
#define SRVM_TOTEMR_PIN 2 /* second totem */
/** Table for the time spend by each servo in high position. */
-volatile uint8_t servo_time[3] = { SRVM_POS_IN, SRVM_POS_IN, SRVM_POS_IN };
+volatile uint8_t servo_time[3] = { SRVM_TRASH_POS_CLOSE, SRVM_POS_IN, SRVM_POS_IN };
/** Different states of this module. */
-enum ServoMotorState
-{
- sleeping = 0,
- trash,
- totem_1,
- totem_2
-};
+
+#define SRVM_STATE_SLEEP 0
+#define SRVM_STATE_TRASH 1
+#define SRVM_STATE_TOTEM1 2
+#define SRVM_STATE_TOTEM2 3
+
/** State variable. */
-volatile enum ServoMotorState servo_motor_state;
+volatile uint8_t servo_motor_state;
/** Time the TC2 have to sleep after manage all the servo motor. */
volatile uint16_t srvm_need_to_sleep;
@@ -63,7 +62,7 @@ servo_motor_init (void)
DDRB |= _BV (SRVM_TRASH_PIN) | _BV (SRVM_TOTEML_PIN) |
_BV (SRVM_TOTEMR_PIN);
/* We are sleeping */
- servo_motor_state = sleeping;
+ servo_motor_state = SRVM_STATE_SLEEP;
/* Prescaler 256 => 4.44 ms TOP */
TCCR2 = regv (FOC2, WGM20, COM21, COM20, WGM21, CS22, CS21, CS20,
0, 0, 0, 0, 0, 1, 0, 0);
@@ -84,8 +83,14 @@ servo_motor_set_pos (uint8_t servo_num, uint8_t servo_pos)
{
/* Set the value and bound it */
servo_time[num] = servo_pos;
- UTILS_BOUND (servo_time[num], SRVM_POS_IN,
- SRVM_POS_OUT);
+ /* Trash */
+ if (num == 0)
+ UTILS_BOUND (servo_time[num], SRVM_TRASH_POS_OPEN,
+ SRVM_TRASH_POS_CLOSE);
+ /* Totem* */
+ else
+ UTILS_BOUND (servo_time[num], SRVM_POS_IN,
+ SRVM_POS_OUT);
}
}
@@ -103,7 +108,7 @@ SIGNAL (SIG_OVERFLOW2)
switch (servo_motor_state)
{
- case sleeping:
+ case SRVM_STATE_SLEEP:
/* We remove from the TOP the number of TIC of this servo */
TCNT2 = TC2_TOP - servo_time[servo_motor_state];
/* We activate the pin */
@@ -111,9 +116,9 @@ SIGNAL (SIG_OVERFLOW2)
/* We remove the TIC of this servo from the main one */
srvm_need_to_sleep -= servo_time[servo_motor_state];
/* Next state */
- servo_motor_state = trash;
+ servo_motor_state = SRVM_STATE_TRASH;
break;
- case trash:
+ case SRVM_STATE_TRASH:
/* We remove from the TOP the number of TIC of this servo */
TCNT2 = TC2_TOP - servo_time[servo_motor_state];
/* Unactivate previous pin */
@@ -123,9 +128,9 @@ SIGNAL (SIG_OVERFLOW2)
/* We remove the TIC of this servo from the main one */
srvm_need_to_sleep -= servo_time[servo_motor_state];
/* Next state */
- servo_motor_state = totem_1;
+ servo_motor_state = SRVM_STATE_TOTEM1;
break;
- case totem_1:
+ case SRVM_STATE_TOTEM1:
/* We remove from the TOP the number of TIC of this servo */
TCNT2 = TC2_TOP - servo_time[servo_motor_state];
/* Unactivate previous pin */
@@ -135,9 +140,9 @@ SIGNAL (SIG_OVERFLOW2)
/* We remove the TIC of this servo from the main one */
srvm_need_to_sleep -= servo_time[servo_motor_state];
/* Next state */
- servo_motor_state = totem_2;
+ servo_motor_state = SRVM_STATE_TOTEM2;
break;
- case totem_2:
+ case SRVM_STATE_TOTEM2:
if (srvm_ov_for_sleeping == -1)
{
/* We need to wait some TIC */
@@ -152,7 +157,7 @@ SIGNAL (SIG_OVERFLOW2)
if (--srvm_ov_for_sleeping == -1)
{
/* Next state */
- servo_motor_state = sleeping;
+ servo_motor_state = SRVM_STATE_SLEEP;
/* Re init */
srvm_need_to_sleep = SRVM_CYCLE_DELAY;
}
@@ -160,3 +165,17 @@ SIGNAL (SIG_OVERFLOW2)
break;
}
}
+
+/** Open the trash. */
+void
+servo_motor_open_trash (void)
+{
+ servo_motor_set_pos (1, SRVM_TRASH_POS_OPEN);
+}
+
+/** Close the trash. */
+void
+servo_motor_close_trash (void)
+{
+ servo_motor_set_pos (1, SRVM_TRASH_POS_CLOSE);
+}
diff --git a/n/es-2006/src/servo_motor.h b/n/es-2006/src/servo_motor.h
index ebece15..f14afc9 100644
--- a/n/es-2006/src/servo_motor.h
+++ b/n/es-2006/src/servo_motor.h
@@ -34,6 +34,9 @@
#define SRVM_POS_OUT 143
#define SRVM_POS_MIDDLE 91
+#define SRVM_TRASH_POS_OPEN 68 /* 44 */
+#define SRVM_TRASH_POS_CLOSE 100 /* 64 */
+
/** Init the servo motor system. */
void servo_motor_init (void);
@@ -49,4 +52,10 @@ void servo_motor_set_pos (uint8_t servo_nun, uint8_t servo_pos);
*/
uint8_t servo_motor_get_pos (uint8_t servo_num);
+/** Open the trash. */
+void servo_motor_open_trash (void);
+
+/** Close the trash. */
+void servo_motor_close_trash (void);
+
#endif // servo_motor_h
diff --git a/n/es-2006/src/sharp.c b/n/es-2006/src/sharp.c
new file mode 100644
index 0000000..62b0719
--- /dev/null
+++ b/n/es-2006/src/sharp.c
@@ -0,0 +1,59 @@
+/* sharp.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 "sharp.h"
+
+#include "modules/adc/adc.h"
+#include "modules/utils/utils.h"
+
+/** Array of sharp values. */
+uint16_t sharp_values[SHARP_NUMBER];
+
+/** Init this module. */
+void
+sharp_init (void)
+{
+ /* Init ADC module */
+ adc_init ();
+ /* All pins are on the right direction, nothing to do */
+}
+
+/** Read data from sharp sensors. */
+void
+sharp_update_values (uint8_t sharp_num)
+{
+ /* Check sharp exists (prevent kernel panic) */
+ if (sharp_num < SHARP_NUMBER)
+ {
+ /* Start capture */
+ adc_start (sharp_num + 1);
+ /* Wait ADC to finish the sensor */
+ while (!adc_checkf ())
+ ;
+ /* Wait a little */
+ utils_delay_us (250);
+ /* Get values */
+ sharp_values[sharp_num] = adc_read ();
+ }
+}
diff --git a/n/es-2006/src/sharp.h b/n/es-2006/src/sharp.h
new file mode 100644
index 0000000..03827d7
--- /dev/null
+++ b/n/es-2006/src/sharp.h
@@ -0,0 +1,54 @@
+#ifndef sharp_h
+#define sharp_h
+// sharp.h
+// 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.
+//
+// }}}
+/** Module for reading sharp sensors. */
+
+#include "common.h"
+
+/** Number of sharps. */
+#define SHARP_NUMBER 3
+
+/** Array of sharp values. */
+extern uint16_t sharp_values[SHARP_NUMBER];
+
+/** Init this module. */
+void sharp_init (void);
+
+/** Read data from sharp sensors.
+ * - sharp_num : the sharp number you want to update.
+ */
+void sharp_update_values (uint8_t sharp_num);
+
+/** Analyse a sharp value.
+ * - sharp_num : the sharp number you want to update.
+ * - return : 0 if nothing in front, 1 otherwise.
+ */
+uint8_t sharp_analyse_values (uint8_t sharp_num);
+
+/** Configure sharp threshold.
+ */
+
+#endif // sharp_h
diff --git a/n/es-2006/src/sniff_rvb.c b/n/es-2006/src/sniff_rvb.c
index fa48498..e5cc765 100644
--- a/n/es-2006/src/sniff_rvb.c
+++ b/n/es-2006/src/sniff_rvb.c
@@ -39,8 +39,8 @@ uint8_t sniff_rvb_ref_ratio_ = 3;
/* test with new robot clothes */
/* ball black/white detection */
-uint16_t green_limit_ = 2800;
-uint16_t clear_limit_ = 3200;
+uint16_t green_limit_ = 3700;
+uint16_t clear_limit_ = 4000;
/** Configure the sensor analysis system. */
void
@@ -83,11 +83,17 @@ sniff_rvb_try_reference (uint8_t sensor_num, uint16_t
uint8_t
sniff_rvb_analysis_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;
+ /* If sensor is valid */
+ if (sensor_rvb_values[sensor][0] < RVB_INVALID_CAPTURE)
+ {
+ // XXX We can use all four values, and not only two ?
+ 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;
+ }
+ return 0;
}
/* Test blue or red */
@@ -97,7 +103,7 @@ 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])
+ if (sensor_rvb_values[sensor][RVB_INDEX_CLEAR] < sniff_rvb_reference_color[sensor][RVB_INDEX_CLEAR])
what_is_it = RVB_SNIFF_BLUE;
}
else if (color == RVB_SNIFF_RED)
@@ -130,20 +136,21 @@ sniff_rvb_analysis_color (uint8_t sensor, uint8_t mode)
return what_is_it;
/* test for red */
- if (sniff_rvb_analysis_color_other (sensor, RVB_SNIFF_RED))
+ if (sniff_rvb_analysis_color_other (sensor, RVB_SNIFF_RED)
+ == 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))
+ if (sniff_rvb_analysis_color_other (sensor, RVB_SNIFF_BLUE)
+ == 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;
+ return RVB_SNIFF_OTHER;
}