/* 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é */ /* etat pour l'interruption fourche optique montee de balle arriere */ #define MONTEE 1 #define DESCENTE 0 #define TERMINE 0 #define EN_COURS 1 /* XXX XXX passer en 16 bits */ /* vitesse de rotation maximale du barillet */ #define VITESSE_BAR_MAX_ 0x015F /* vitesse de rotation minimale du barillet */ #define VITESSE_BAR_MIN_ 0xA0 /* 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 // etat d'attente que la PC104 nous dise GO #define STAND_BY_ 1 #define BARRIERE_AVANT_ACTIVEE_ 2 #define EXTRACTION_DEPOT_ 3 #define ROTATION_BARILLET_ 4 #define ROTATION_EN_COURS_ 5 /* varaible 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_; /* 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 pour le delay aspiration */ volatile uint8_t compteur_timer_; /* etat de la rotation */ volatile uint8_t rotation_en_cours_; uint8_t rotation_local_; /* position a aller pour une rotation local */ uint8_t pos_a_aller_; /* variable indiquant que la balle est en cours d'extraction */ volatile uint8_t extraire; /* variable gerant le pb de rebond dans les interruptions */ volatile uint8_t rebond_inter_ar; volatile uint8_t rebond_inter_av; /*** variable globale avec le main ***/ /* variable indiquant qu'il faut faire une extraction de balle */ uint8_t extraction; /* variable indiquant que l'on depose une balle, on attend que la barriere optique voit la balle passee */ uint8_t depot; /* variable pour lancer une rotation */ uint8_t rotation; /* position du barillet a aller */ uint8_t pos_a_aller; /* fonctions static dans le fichier */ static void rotation_barillet( uint8_t pos_a_aller ); static void pos_bar(void); static void extraction_depot(void); static void extraction_balle(void); static void extraction_fin(void); static void new_balle(void); static void depose_balle(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 */ // FIXME // 1µs ok, mais fait pas appel à delay_ms non ? // Passe aussi ton 1 en define, histoire que si tu veuilles le changer, // ca ne soit pas trop galère. Voir meme une fonction inline qui // appelle que le utils_delay_ms, enfin on verra après. // FIXME // il y a pas un _ à la fin ? Met en majuscule, ca sera plus // simple donc. 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 */ pos_prec_ = 0; // XXX penser a mettre le barillet avec un trou vers l'avant du robot XXX pos_temp_ = 0; pos_a_aller = 0; pos_actuel_ = 0; pos_final_ = 0; etat_en_cours_ = SLEEP_; rotation = 0; rotation_en_cours_ = 0; rotation_local_ = 0; compteur_timer_ = 0; extraction = 0; depot = 0; rebond_inter_av = 0; rebond_inter_ar = 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 * * ******************************/ /* fonction main barillet */ void sequenceur_barillet() { if ( etat_en_cours_ != SLEEP_ ) { ++compteur_timer_; if ( etat_en_cours_ == ROTATION_EN_COURS_ ) { //ne rien faire proto_send1b ('f', pos_temp_); // affichage de pos_temp_ et pos_actuel_ } else if ( ( rotation_en_cours_ == 0 ) && ( rotation == 1 ) ) { etat_en_cours_ = ROTATION_BARILLET_; } else if ( ( etat_inter_av_ == MONTEE ) && ( compteur_timer_ > 5 ) ) // XXX verifier le 5, est ce suffisant ? XXX la balle est elle montee depuis longtemps ? { etat_en_cours_ = BARRIERE_AVANT_ACTIVEE_; } else if ( ( etat_inter_ar_ == MONTEE ) || ( etat_inter_ar_ == DESCENTE ) || ( extraction == 1) || ( depot == 1 ) ) { etat_en_cours_ = EXTRACTION_DEPOT_; } } switch(etat_en_cours_) { case SLEEP_ : break; // XXX cette partie sera gerer autrement quand le main 'es' sera finie case STAND_BY_ : break; case BARRIERE_AVANT_ACTIVEE_ : new_balle(); break; case EXTRACTION_DEPOT_ : extraction_depot(); break; case ROTATION_BARILLET_ : rotation_barillet(pos_a_aller); break; case ROTATION_EN_COURS_ : break; default : break; } } /* ********************** * ROTATION DU BARILLET * * **********************/ void rotation_barillet( uint8_t pos_a_aller ) { /* initialisation pour la rotation */ pos_final_ = ( pos_a_aller * 4 ); /* affecter le bon sens de rotation */ /* if ( ( ( ( (40 + pos_actuel_ - pos_final_) % 40 ) >= 10 && ( pos_final_ < 10 ) ) ) || ( ( ( (40 + pos_actuel_ - pos_final_) % 40 ) < 10 && ( pos_final_ > 10 ) ) ) ) { PORTB &= ~_BV(4); utils_delay_us ( DELAY_ ); } else // ( pos_final < 20 ) { PORTB |= _BV(4); utils_delay_us ( DELAY_ ); } */ vitesse_turbine ( 1,VITESSE_TURB_MIN_); /* tourner le barillet */ // if ( ( ( ( 40 + pos_actuel_ - pos_final_) % 40 ) < 5 ) || // ( ( ( 40 + pos_actuel_ - pos_final_) % 40 ) > 35 ) ) // { // OCR1A = VITESSE_BAR_MIN_; // } // else // { OCR1A = VITESSE_BAR_MAX_; // } etat_en_cours_ = ROTATION_EN_COURS_; } /* 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_; etat_en_cours_ = ROTATION_EN_COURS_; switch ( pos_conc_ ) { // actu pres case 1 : // 00 01 case 7 : // 01 11 case 8 : // 10 00 case 14 : // 11 10 --pos_actuel_; 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_temp_ == 3 ) && ( pos_actuel_ == pos_final_ ) ) if ( pos_actuel_ == ( pos_final_ - 2 )) // +/- { OCR1A = 0; } /* if ( ( pos_actuel_ == pos_final_ ) && ( pos_actuel_ == 3 ) ) { OCR1A = 0; rotation_en_cours_ = TERMINE; rotation_local_ = TERMINE; vitesse_turbine ( 1, VITESSE_BAR_MAX_); rotation = TERMINE; 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 * * ******************************/ void extraction_depot(void) { rebond_inter_av = 0; rebond_inter_ar = 0; if ( extraction == 1 ) { if ( extraire == TERMINE ) // debut de l'extraction { extraction_balle(); } else // fin de l'extraction { extraction_fin(); } } else if ( depot == 1 ) { depose_balle(); } } /* Si balle dans trou => Extraction */ void extraction_balle(void) { /* Ralentir la turbine avant */ vitesse_turbine( 2, VITESSE_TURB_MIN_); /* Vitesse max de la turbine arriere */ vitesse_turbine( 1, VITESSE_TURB_MAX_); extraire = EN_COURS; } /* fin d'extration de balle a appeller si extraction_balle =1 et interrupt arriere =1 */ void extraction_fin(void) { /* Ralentir la vitesse de la turbine arriere pour la rotation */ vitesse_turbine( 1, VITESSE_TURB_MIN_); /* fin d'extraction */ extraction = TERMINE; extraire = TERMINE; /* rotation local */ pos_a_aller_ = ( pos_prec_ + 2) % 10; rotation_local_ = EN_COURS; etat_en_cours_ = ROTATION_BARILLET_; } /* deposer une balle dans un trou apres rotation */ void depose_balle(void) { /* attente de l'interruption */ etat_inter_ar_ = DESCENTE; /* vitesse turbine arriere nulle */ vitesse_turbine( 2, 0); } /* *********************** * MONTEE DE BALLE AVANT * * ***********************/ void new_balle(void) { /* baisser la vitesse de la turbine */ OCR1B = VITESSE_TURB_MIN_; if ( (PINE & 64 ) == 0 ) /* le Port E bit 6 est a 0 => plus de balle devant la barriere optique */ { /* faire tourner le barillet pour voir la couleur */ rotation_barillet( pos_prec_ + 8 ); } else if ( rotation_en_cours_ == TERMINE ) { /* XXX appel de la fonction pour lire la couleur de la balle */ /* ca y'ai, on a fini, remettre la variable etat_inter_av_ a 0 */ etat_inter_av_ = DESCENTE; //interruption terminee } } /* *************** * INTERRUPTIONS * * ***************/ /* interruption fourche optique turbine avant */ SIGNAL ( SIG_INTERRUPT6 ) { if ( etat_en_cours_ != SLEEP_ ) { if ( rebond_inter_av == 0 ) { if(etat_en_cours_ == STAND_BY_) { etat_inter_av_ = MONTEE; compteur_timer_ = 0; } } //proto_send0('A'); } } /* interruption fourche optique turbine arriere */ SIGNAL ( SIG_INTERRUPT7 ) { if ( etat_en_cours_ != SLEEP_ ) { if ( rebond_inter_ar == 0 ) { rebond_inter_ar = 1; if ( extraire == EN_COURS ) { etat_inter_ar_ = MONTEE; compteur_timer_ = 0; } else if ( etat_inter_ar_ == DESCENTE ) { /* remettre la vitesse turbine a min */ vitesse_turbine( 1, VITESSE_TURB_MIN_); /* XXX verifier que la balle ne remonte pas */ /* XXX mettre une variable pour dire : OK, on peut avancer ??*/ } // sinon c'est que la barriere s'est déclanché dans un cas non voulu //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 XXX XXX XXX gerer pb des rebond dans les iterruptions pour les barrieres optiques */