From 4d1bbfbad04a9af0436fc1709ac301f19fea9260 Mon Sep 17 00:00:00 2001 From: dufourj Date: Tue, 23 May 2006 08:47:34 +0000 Subject: 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. --- n/es-2006/src/Makefile | 5 +- n/es-2006/src/avrconfig.h | 8 ++- n/es-2006/src/barillet.h | 3 ++ n/es-2006/src/lcd.c | 56 +++++++++++++++++++ n/es-2006/src/lcd.h | 45 ++++++++++++++++ n/es-2006/src/main.c | 128 +++++++++++++++++++++++++++++++++++++------- n/es-2006/src/others.h | 40 +++++++------- n/es-2006/src/sensor_rvb.c | 33 ++++++++---- n/es-2006/src/servo_motor.c | 59 +++++++++++++------- n/es-2006/src/servo_motor.h | 9 ++++ n/es-2006/src/sharp.c | 59 ++++++++++++++++++++ n/es-2006/src/sharp.h | 54 +++++++++++++++++++ n/es-2006/src/sniff_rvb.c | 31 ++++++----- 13 files changed, 447 insertions(+), 83 deletions(-) create mode 100644 n/es-2006/src/lcd.c create mode 100644 n/es-2006/src/lcd.h create mode 100644 n/es-2006/src/sharp.c create mode 100644 n/es-2006/src/sharp.h (limited to 'n') 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; } -- cgit v1.2.3