/* barillet.c */ /* es - Input/Output general purpose board. {{{ * * Copyright (C) 2006 Lambert Thomas * * Robot APB Team/Efrei 2006. * 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 "barillet.h" #include "modules/utils/utils.h" #include "common.h" #include "io.h" #include "modules/proto/proto.h" /* utilisation de OCR1A pour le moteur barillet OCR1B pour la turbine 1 => avant OCR1C pour la turbine 2 => arriere SIG_INTERRUPT7 pour la fourche montee de balle arriere SIG_INTERRUPT6 pour la fourche montee de balle avant SIG_INTERRUPT5 pour la fourche barillet 1 => celle pres de la carte de puissance SIG_INTERRUPT4 pour la fourche barillet 2 => celle pres de la carte de es-2006 */ /* XXX verifier que les turbines sont en vitesse lente si inutilisé */ /* XXX verifier que le sens de rotation du barillet est 0 si inutilisé */ /* XXX XXX passer en 16 bits */ /* vitesse de rotation maximale du barillet */ #define VITESSE_BAR_MAX_ 0x03FF /* vitesse de rotation minimale du barillet */ #define VITESSE_BAR_MIN_ 0x1FF /* XXX a etalonner */ /* vitesse de rotation maximale des turbines */ #define VITESSE_TURB_MAX_ 0x0333 /* vitesse de rotation minimale des turbines */ #define VITESSE_TURB_MIN_ 0xA0 /* XXX a etalonner */ /* delai d'une µs pour la carte puissance */ #define DELAY_ 1 /* define de l'etat_en_cours_ */ #define SLEEP_ 0 #define STAND_BY_ 1 #define BALLE_AVALE_ATTENTE 2 #define BALLE_AVALEE 3 #define TOURNE_ANALYSE_BALLE 4 #define ANALYSE_BALLE 5 #define ROTATION 6 #define ROTATION_EN_COURS 7 #define EXTRACTION 8 #define EXTRACTION_EN_COURS 9 #define EXTRACTION_EN_HAUT 10 #define DEPOT 11 #define DEPOT_EN_COURS 12 #define DEPOT_FIN 13 /* variable d'état_barillet */ volatile uint8_t etat_en_cours_; /*XXX XXX variable de debug pour la rotation barillet */ volatile uint8_t pos_temp_; /* position relative au barillet */ volatile uint8_t pos_actuel_; volatile uint8_t pos_prec_; volatile uint8_t pos_final_; volatile uint8_t pos_lenteur_; /* etat d'attente pour l'interruption fourche optique */ volatile uint8_t etat_inter_ar_; /* fourche arriere */ volatile uint8_t etat_inter_av_; /* fourche avant */ /* variable d'attente pour que la balle soit completemebnt montee dnas le barillet */ volatile uint8_t attente; /* variable gerant le pb de rebond dans les interruptions */ volatile uint8_t rebond_inter_ar; volatile uint8_t rebond_inter_av; /* position du barillet a aller */ uint8_t pos_a_aller_; /* fonctions static dans le fichier */ static void rotation_barillet( void ); static void pos_bar(void); static void extraction_balle(void); static void extraction_fin(void); static void new_balle(void); static void depose_balle(void); static void depot_fin(void); /* Initialisation pour tout ce qui concerne le barillet */ inline void barillet_init (void) { /* initialisation des registres du timer 1 pour la PWM total*/ ///******************************* // * a inserer dans es_config * // * *****************************/ // // TCCR1A = // regv (COM1A1, COM1A0, COM1B1, COM1B0, COM1C1, COM1C0, WGM11, WGM10, // 1, 0, 1, 0, 1, 0, 1, 1); // // TCCR1B = // regv ( ICNC1, ICES1, 5, WGM13, WGM12, CS12, CS11, CS10, // 0, 0, 0, 0, 1, 0, 0, 1); // ///********************************/ /* initialisation des sorties sur le port B pour moteur et turbines */ DDRB |= _BV(7) | _BV(6) | _BV (5) | _BV (4); /* initialisation des entrees du port E pour les interruptions */ DDRE &= ~(_BV(7) | _BV(6) | _BV (5) | _BV (4)); PORTE |= _BV(7) | _BV(6); /* mode PWM = 0 */ OCR1A = 0; /* vitesse moteur barillet nulle */ utils_delay_us ( DELAY_ ); /* attente d'1 µs pour la puiss_barillet */ OCR1B = 0; /* vitesse turb nulle */ OCR1C = 0; /* vitesse turb nulle */ /* initialisation sens rotation */ PORTB &= ~_BV(4); /* interruption autorisees INT7 INT6 INT5 INT4 à chaque changement logique */ EICRB = regv ( ISC71, ISC70, ISC61, ISC60, ISC51, ISC50, ISC41, ISC40, 1, 0, 1, 0, 0, 1, 0, 1); EIFR |= _BV(7) | _BV(6) | _BV(5) | _BV(4); EIMSK |= _BV(7) | _BV(6) | _BV(5) | _BV(4); /* initialisation de la position du barillet */ // XXX penser a mettre le barillet avec un trou vers l'avant du robot XXX pos_prec_ = ( PINE >> 4) & 3; // 00 00 00 XX pos_temp_ = pos_prec_; pos_a_aller_ = 0; pos_actuel_ = 0; pos_final_ = 0; pos_lenteur_ = 0; etat_en_cours_ = SLEEP_; rebond_inter_av = 0; rebond_inter_ar = 0; attente = 0; } /*************************** * init quand le jack part * XXX changer le nom et gerer autrament * *************************/ void init_2_barillet(void) { /* commencer a faire tourner les turbines */ OCR1B = VITESSE_TURB_MAX_; OCR1C = VITESSE_TURB_MIN_; /* passer a l'etat suivant */ etat_en_cours_ = STAND_BY_; } /******************************** * fonction appelee dans le main * * ******************************/ /* demande de rotation */ void rotation( uint8_t pos_fin ) { pos_a_aller_ = pos_fin; if ( etat_en_cours_ == STAND_BY_ ) { etat_en_cours_ = ROTATION; } } /* demande d'extraction */ void extraction ( void ) { if ( etat_en_cours_ == STAND_BY_ ) { etat_en_cours_ = EXTRACTION; } } /* demande de depot */ void depot_balle ( void ) { if ( etat_en_cours_ == STAND_BY_ ) { etat_en_cours_ = DEPOT; } } /* fonction main barillet */ void sequenceur_barillet() { static uint8_t old_pos_actuel = 0; if (old_pos_actuel != pos_actuel_) { proto_send2b('W', pos_actuel_, pos_temp_); old_pos_actuel = pos_actuel_; } switch(etat_en_cours_) { case SLEEP_ : break; case STAND_BY_ : break; case BALLE_AVALE_ATTENTE : new_balle(); break; case TOURNE_ANALYSE_BALLE : // faire une rotation pour checker la couleur rotation_barillet( ); break; case ANALYSE_BALLE : // lancer la fonction pour l'analyse balle break; case ROTATION : //appel de la fonction rotation_barillet rotation_barillet(); break; case ROTATION_EN_COURS : break; case EXTRACTION : //debut de l'extraction extraction_balle(); break; case EXTRACTION_EN_COURS : break; case EXTRACTION_EN_HAUT : extraction_fin(); break; case DEPOT : depose_balle(); break; case DEPOT_EN_COURS : break; case DEPOT_FIN : depot_fin(); break; default : break; } } /* ********************** * ROTATION DU BARILLET * * **********************/ void rotation_barillet(void) { uint8_t dist_trigo, dist_pas_trigo; pos_final_ = pos_a_aller_; if (pos_final_ != pos_actuel_) { /* affecter le bon sens de rotation */ dist_trigo = (40 + pos_final_ - pos_actuel_) % 40; dist_pas_trigo = (40 + pos_actuel_ - pos_final_) % 40; if ( dist_trigo < dist_pas_trigo ) { PORTB &= ~_BV(4); pos_lenteur_ = (pos_final_ + (40 - 2)) % 40; } else { PORTB |= _BV(4); pos_lenteur_ = (pos_final_ + 2) % 40; } utils_delay_us ( DELAY_ ); vitesse_turbine ( 1,VITESSE_TURB_MIN_); vitesse_turbine ( 2,VITESSE_TURB_MIN_); /* affectation de la vitesse du barillet */ if ( ( dist_pas_trigo < 4 ) || ( dist_trigo < 4 ) ) { OCR1A = VITESSE_BAR_MIN_; } else { OCR1A = VITESSE_BAR_MAX_; } etat_en_cours_ = ROTATION_EN_COURS; } else { etat_en_cours_ = STAND_BY_; } } /* gestion de la position du barillet */ void pos_bar(void) { uint8_t pos_conc_; // uint8_t pos_temp_; // proto_send0('Z'); //debug interrupt pos_temp_ = ( PINE >> 4) & 3; // 00 00 00 XX pos_conc_ = ( pos_temp_ << 2 ) | pos_prec_; switch ( pos_conc_ ) { // actu pres case 1 : // 00 01 case 7 : // 01 11 case 8 : // 10 00 case 14 : // 11 10 if ( pos_actuel_ == 0 ) { pos_actuel_ = 40; } --pos_actuel_; break; case 2 : // 00 10 case 4 : // 01 00 case 11 : // 10 11 case 13 : // 11 01 ++pos_actuel_; if ( pos_actuel_ == 40 ) { pos_actuel_ = 0; } break; default : break; } if ( pos_actuel_ == pos_lenteur_ ) { OCR1A = VITESSE_BAR_MIN_; } else if ( pos_actuel_ == pos_final_ ) { OCR1A = 0; vitesse_turbine ( 1, VITESSE_TURB_MAX_); /* XXX a changer, il faut attendre que la PC104 nous disent : ok tu peux remettre le ventillo avant a fond, donc ce sera pas a mettre ici*/ etat_en_cours_ = STAND_BY_; } pos_prec_ = pos_temp_; } /************************************** * CHANGEMENT DE VITESSE DES TURBINES * **************************************/ void vitesse_turbine(uint8_t turbine, uint16_t vitesse) { if ( turbine == 1 ) { OCR1B = vitesse; } else if ( turbine == 2 ) { OCR1C = vitesse; } else if ( turbine == 3 ) // moteur barillet { OCR1A = vitesse; } } /**** changement de sens ****/ void sens(uint8_t sens_rotat) { if ( sens_rotat == 0 ) { PORTB |= _BV(4); } else if ( sens_rotat == 1 ) { PORTB &= ~_BV(4); } } /* ****************************** * EXTRACTION ET DEPOT DE BALLE * * ******************************/ /* Si balle dans trou => Extraction */ void extraction_balle(void) { rebond_inter_ar = 0; /* Ralentir la turbine avant */ vitesse_turbine( 1, VITESSE_TURB_MIN_); /* Vitesse max de la turbine arriere */ vitesse_turbine( 2, VITESSE_TURB_MAX_); etat_en_cours_ = EXTRACTION_EN_COURS; } /* fin d'extration de balle */ void extraction_fin(void) { if ( (PINE & 128 ) == 0 ) /* le Port E bit 7 est a 0 => plus de balle devant la barriere optique */ { ++attente; /* attendre que la balle soit bien montee */ if ( attente >= 10 ) { rebond_inter_ar = 0; vitesse_turbine( 2, VITESSE_TURB_MIN_); /* fin d'extraction - faire la rotation */ pos_a_aller_ = ( 40 + pos_actuel_ + 8 ) % 40; etat_en_cours_ = TOURNE_ANALYSE_BALLE; } } else { // XXX que faire ??? } } /* deposer une balle dans un trou apres rotation */ void depose_balle(void) { rebond_inter_ar = 0; /* vitesse turbine arriere nulle */ vitesse_turbine( 2, 0); etat_en_cours_ = DEPOT_EN_COURS; } /* fin de la depose de balle */ void depot_fin(void) { if ( (PINE & 128 ) == 0 ) /* le Port E bit 6 est a 0 => plus de balle devant la barriere optique */ { ++attente; /* attendre que la balle soit bien montee */ if ( attente >= 10 ) { etat_inter_ar_ = 0; vitesse_turbine( 2, VITESSE_BAR_MIN_); etat_en_cours_ = STAND_BY_; } } else { /* XXX que faire */ } } /* *********************** * MONTEE DE BALLE AVANT * * ***********************/ void new_balle(void) { if ( (PINE & 64 ) == 0 ) /* le Port E bit 6 est a 0 => plus de balle devant la barriere optique */ { ++attente; /* attendre que la balle soit bien montee */ if ( attente >= 10 ) { rebond_inter_av = 0; /* faire tourner le barillet pour voir la couleur */ pos_a_aller_ = ( 40 + pos_actuel_ + 8 ) % 40; etat_en_cours_ = TOURNE_ANALYSE_BALLE; } } } /* *************** * INTERRUPTIONS * * ***************/ /* interruption fourche optique turbine avant */ SIGNAL ( SIG_INTERRUPT6 ) { if ( etat_en_cours_ != SLEEP_ ) { if ( rebond_inter_av == 0 ) { rebond_inter_av == 1; OCR1B = VITESSE_TURB_MIN_; attente = 0; etat_en_cours_ = BALLE_AVALE_ATTENTE; //proto_send0('A'); } } } /* interruption fourche optique turbine arriere */ SIGNAL ( SIG_INTERRUPT7 ) { if ( etat_en_cours_ != SLEEP_ ) { if ( rebond_inter_ar == 0 ) { attente = 0; rebond_inter_ar = 1; if ( etat_en_cours_ == EXTRACTION_EN_COURS ) { etat_en_cours_ = EXTRACTION_EN_HAUT; } else if ( etat_en_cours_ == DEPOT_EN_COURS ) { /* remettre la vitesse turbine a min */ vitesse_turbine( 2, VITESSE_TURB_MIN_); /* XXX verifier que la balle ne remonte pas */ etat_en_cours_ = DEPOT_FIN; } //proto_send0('B'); } } } /* interruption fourche optique barillet 1 */ SIGNAL ( SIG_INTERRUPT5 ) { //proto_send0('A'); if ( etat_en_cours_ != SLEEP_ ) { pos_bar(); } } /* interruption fourche optique barillet 1 */ SIGNAL ( SIG_INTERRUPT4 ) { //proto_send0('B'); if ( etat_en_cours_ != SLEEP_ ) { pos_bar(); } } /* XXX dans la PC104, apres un depot, il faudra remmetre le barillet dans une des positions 00, 08, 10, 18, 20 ( position exa ) */