/* 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" #include "ack.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 2 => celle pres de la carte de puissance SIG_INTERRUPT4 pour la fourche barillet 1 => celle pres de la carte de es-2006 */ #define OCR_BAR OCR1A #define OCR_TURB_AVANT OCR1B #define OCR_TURB_ARRIERE OCR1C /* 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_ 0x01FF /* XXX a etalonner */ /* vitesse de rotation maximale des turbines */ #define VITESSE_TURB_MAX_ 0x0333 /* vitesse de rotation minimale des turbines */ #define VITESSE_TURB_MIN_ 0x0100 /* delai d'une µs pour la carte puissance */ #define DELAY_ 1 /* delai de 500 µs pour la vidange */ #define DELAY_VIDANGE_ 500 /* Define de l'etat_en_cours_. * Lexique : * AV : trou avant. * AR : trou arrière. * EX : extraction/montée. * DEP : depot/descente. * sans rien (ex AR_EX) : on a reçu un ordre, mais on n'a pas encore vu de * balle. * VU : balle devant le capteur. * PASSE : balle n'est plus devant le capteur, on attend qu'elle arrive à * destination (tombe ou monte). * FIN : opération réalisée avec succès, on l'indique à l'uart. */ #define ETAT_ROTATION 0x80 /** Retourne une valeur non nulle si c'est une rotation. */ #define EST_CE_UNE_ROTATION(e) ((e) & ETAT_ROTATION) /** Passe à l'état suivant si on est dans un etat rotation. */ #define ETAT_SUIVANT_ROTATION(e) (((e) & ~ETAT_ROTATION) + 1) /** Etat initial au reset. */ #define RESET (0x00) /** Initialisation de la position zero du barillet. */ #define INIT (0x01) /** Attente sans rien faire. */ #define SLEEP (0x02) /** Prêt à agir. */ #define STAND_BY (0x03) /** Prêt à agir sauf pour faire monter des balles. les turbines sont au minimum */ #define STAND_BY_FULL (0x04) /** il faudra remettre les turbines au minimum */ #define STAND_BY_ARRET (0x05) #define ROTATION (0x10 | ETAT_ROTATION) #define ROTATION_FIN (0x11) #define AV_EX_VU (0x20) #define AV_EX_PASSE (0x21) #define AV_EX_SOLO (0x22) #define AV_EX_ROTATION (0x23 | ETAT_ROTATION) #define AV_EX_FIN (0x24) #define AR_EX (0x30) #define AR_EX_VU (0x31) #define AR_EX_PASSE (0x32) #define AR_EX_ROTATION (0x33 | ETAT_ROTATION) #define AR_EX_FIN (0x34) #define AR_DEP (0x40) #define AR_DEP_VU (0x41) #define AR_DEP_PASSE (0x42) #define AR_DEP_FIN (0x43) #define VIDANGE_1 (0x50 | ETAT_ROTATION) #define VIDANGE_2 (0x51) #define VIDANGE_3 (0x52 | ETAT_ROTATION) #define VIDANGE_4 (0x53) #define VIDANGE_5 (0x54) /* at barillet. */ volatile uint8_t etat_en_cours_; /* Etat des fourches barillet. */ volatile uint8_t fourche_barillet_prec_; /* position relative au barillet */ volatile uint8_t pos_actuel_; volatile uint8_t pos_final_; volatile uint8_t pos_lenteur_; /* variable de vidange */ uint8_t vidange_; /* compteur de vidange */ uint8_t compt_vidange; /* Compteur utilisé pour les time out ou les passages de balle. */ volatile uint16_t attente_; /* fonctions static dans le fichier */ static void rotation_barillet (uint8_t pos_final, uint8_t etat); static void pos_bar(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 */ OCR_BAR = 0; /* vitesse moteur barillet nulle */ utils_delay_us ( DELAY_ ); /* attente_ d'1 µs pour la puiss_barillet */ OCR_TURB_AVANT = 0; /* vitesse turb nulle */ OCR_TURB_ARRIERE = 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, 0, 1, 0, 1, 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_actuel_ = 0; pos_final_ = 0; pos_lenteur_ = 0; etat_en_cours_ = RESET; attente_ = 0; fourche_barillet_prec_ = (PINE >> 4) & 3; vidange_ = 0; compt_vidange = 0; } /******************************** * fonction appelee dans le main * * ******************************/ /** Initialise la position zero du barillet. */ void barillet_init_zero (void) { if (etat_en_cours_ == RESET) { OCR_BAR = VITESSE_BAR_MIN_; etat_en_cours_ = INIT; } } /* Lancement des turbines en vitesse lente */ void barillet_debut_lancement (void) { if (etat_en_cours_ == SLEEP || etat_en_cours_ == STAND_BY || etat_en_cours_ == STAND_BY_ARRET) { /* Commencer a faire tourner les turbines. */ OCR_TURB_AVANT = VITESSE_TURB_MIN_; OCR_TURB_ARRIERE = VITESSE_TURB_MIN_; etat_en_cours_ = STAND_BY_FULL; } } /* Lance la procédure de collecte de toutes les balles de la table. */ void barillet_lancement (void) { if (etat_en_cours_ == STAND_BY_FULL) { /* Commencer a faire tourner les turbines. */ OCR_TURB_AVANT = VITESSE_TURB_MAX_; OCR_TURB_ARRIERE = VITESSE_TURB_MIN_; etat_en_cours_ = STAND_BY; } } /* Dodo. - fin du match - */ void barillet_sleep (void) { if (etat_en_cours_ == STAND_BY || etat_en_cours_ == STAND_BY_FULL) { OCR_TURB_AVANT = 0; OCR_TURB_ARRIERE = 0; } } /* Demande de rotation. */ void rotation (uint8_t pos_final) { if (etat_en_cours_ == STAND_BY || etat_en_cours_ == STAND_BY_FULL) rotation_barillet (pos_final, ROTATION); } /* Demande d'extraction. */ void extraction (void) { if (etat_en_cours_ == STAND_BY || etat_en_cours_ == STAND_BY_FULL) { etat_en_cours_ = AR_EX; OCR_TURB_AVANT = VITESSE_TURB_MIN_; OCR_TURB_ARRIERE = VITESSE_TURB_MAX_; } } /* Demande de depot. */ void depot_balle (void) { if (etat_en_cours_ == STAND_BY || etat_en_cours_ == STAND_BY_FULL) { etat_en_cours_ = AR_DEP; OCR_TURB_AVANT = VITESSE_TURB_MIN_; OCR_TURB_ARRIERE = 0; attente_ = 200; } } /* vidange total du barillet */ void vidange_barillet(void) { if ( ( etat_en_cours_ == STAND_BY || etat_en_cours_ == STAND_BY_FULL ) ) { compt_vidange = 0; etat_en_cours_ = VIDANGE_1; rotation_barillet( 0x26, VIDANGE_1 ); } } void sequenceur_barillet() { /* static uint8_t old_pos_actuel = 0; if (old_pos_actuel != pos_actuel_) { proto_send2b('W', pos_actuel_, fourche_barillet_prec_); old_pos_actuel = pos_actuel_; } */ if (attente_) { attente_--; return; } switch(etat_en_cours_) { case RESET: break; case SLEEP: break; case STAND_BY: break; case STAND_BY_FULL: break; case STAND_BY_ARRET: break; case ROTATION_FIN: // XXX ack etat_en_cours_ = STAND_BY_FULL; break; case AV_EX_VU: /* Time out en cas de deux balles dans le trou avant. */ case AV_EX_PASSE: OCR_TURB_AVANT = VITESSE_TURB_MIN_ - 17; if (PINE & _BV(6)) etat_en_cours_ = AV_EX_SOLO; break; case AV_EX_SOLO: ack_set (ACK_BARILLET_BALL_ARRIVED); rotation_barillet ((pos_actuel_ + 8) % 40, AV_EX_ROTATION); break; case AV_EX_FIN: ack_set (ACK_BARILLET_BALL_FRONT); etat_en_cours_ = STAND_BY_FULL; break; case AR_EX: break; case AR_EX_VU: break; case AR_EX_PASSE: ack_set (ACK_BARILLET_BALL_ARRIVED); rotation_barillet ((pos_actuel_ + 8) % 40, AR_EX_ROTATION); break; case AR_EX_FIN: ack_set (ACK_BARILLET_BALL_REAR); etat_en_cours_ = STAND_BY_FULL; break; case AR_DEP: // info PC104 que le timer est arrivé à expiration. etat_en_cours_ = STAND_BY_FULL; break; case AR_DEP_VU: etat_en_cours_ = STAND_BY_FULL; break; case AR_DEP_PASSE: OCR_TURB_ARRIERE = VITESSE_TURB_MIN_; etat_en_cours_ = AR_DEP_FIN; break; case AR_DEP_FIN: etat_en_cours_ = STAND_BY_ARRET; break; /* revoir les fonctions a apeller */ case VIDANGE_1: break; case VIDANGE_2: /* ouverture du servo moteur */ servo_motor_open_trash(); etat_en_cours_ = VIDANGE_3; rotation_barillet (0, VIDANGE_3); break; case VIDANGE_3: /* rotation du barillet en cours */ break; case VIDANGE_4: attente_ = 300; etat_en_cours_ = VIDANGE_5; break; case VIDANGE_5: compt_vidange = compt_vidange + 1; if ( compt_vidange == 5 ) /* vidange terminee */ { servo_motor_close_trash(); etat_en_cours_ = STAND_BY_FULL; } if ( compt_vidange <= 4 ) { etat_en_cours_ = VIDANGE_3; rotation_barillet( ( (pos_actuel_ + 8)%40), VIDANGE_3); } break; default : break; } } /** Démare une rotation du barillet. */ static void rotation_barillet (uint8_t pos_final, uint8_t etat) { uint8_t dist_trigo, dist_pas_trigo; uint8_t pos_actuel; /* Ça a l'air con comme ça, mais en fait la première ligne recopie * l'argument dans la variable globale et la seconde recopie la variable * globale dans la variable locale. Intérêt ? La variable globale est * volatile donc lente à accéder. La variable locale ne l'est pas, donc on * garde sa valeur dans un registre. Cool non ? */ pos_final_ = pos_final; pos_actuel = pos_actuel_; 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) { /* Tourne dans le sens trigo. */ PORTB &= ~_BV(4); pos_lenteur_ = (pos_final + (40 - 2)) % 40; } else { PORTB |= _BV(4); pos_lenteur_ = (pos_final + 2) % 40; } /* Delay pour la commutation du pont en H. */ utils_delay_us (DELAY_); if (etat != AV_EX_ROTATION ) { OCR_TURB_AVANT = VITESSE_TURB_MIN_; } OCR_TURB_ARRIERE = VITESSE_TURB_MIN_; /* Affectation de la vitesse du barillet. */ if ((dist_pas_trigo <= 2) || (dist_trigo <= 2)) { OCR_BAR = VITESSE_BAR_MIN_; } else { OCR_BAR = VITESSE_BAR_MAX_; } etat_en_cours_ = etat; } else etat_en_cours_ = ETAT_SUIVANT_ROTATION (etat); } /* gestion de la position du barillet */ static void pos_bar (void) { uint8_t concat, fourche; /* Ici, on place dans concat une valeur 0000nnoo, avec nn la nouvelle * valeur, et oo l'ancienne valeur des fourches, et on mémorise la * nouvelle valeur pour le prochain passage dans le coin. */ fourche = (PINE >> 4) & 3; concat = (fourche << 2) | fourche_barillet_prec_; fourche_barillet_prec_ = fourche; switch ( concat ) { // actu prec 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; } /* Transitions. */ if (EST_CE_UNE_ROTATION (etat_en_cours_)) { if (pos_actuel_ == pos_lenteur_) { OCR_BAR = VITESSE_BAR_MIN_; } else if (pos_actuel_ == pos_final_) { OCR_BAR = 0; etat_en_cours_ = ETAT_SUIVANT_ROTATION (etat_en_cours_); } } /* Pour l'initialisation, il faut être sur une position ou les deux * fourches sont non coupées, et la troisième fourche aussi. */ else if (etat_en_cours_ == INIT && (fourche == 3) && (PINF & 0x01)) { OCR_BAR = 0; pos_actuel_ = 0; etat_en_cours_ = SLEEP; } } /************************************** * CHANGEMENT DE VITESSE DES TURBINES * **************************************/ void vitesse_turbine(uint8_t turbine, uint16_t vitesse) { if ( turbine == 1 ) { if (vitesse > VITESSE_TURB_MAX_) OCR_TURB_AVANT = VITESSE_TURB_MAX_; else OCR_TURB_AVANT = vitesse; } else if ( turbine == 2 ) { if (vitesse > VITESSE_TURB_MAX_) OCR_TURB_ARRIERE = VITESSE_TURB_MAX_; else OCR_TURB_ARRIERE = vitesse; } else if ( turbine == 3 ) // moteur barillet { if (vitesse > VITESSE_BAR_MAX_) OCR_BAR = VITESSE_BAR_MAX_; else OCR_BAR = 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); } } /* *************** * INTERRUPTIONS * * ***************/ /* interruption fourche optique turbine avant */ SIGNAL (SIG_INTERRUPT6) { if (etat_en_cours_ == STAND_BY) { etat_en_cours_ = AV_EX_VU; attente_ = 500; } else if (etat_en_cours_ == AV_EX_VU) { etat_en_cours_ = AV_EX_PASSE; attente_ = 200; } } /* interruption fourche optique turbine arriere */ SIGNAL (SIG_INTERRUPT7) { if (etat_en_cours_ == AR_EX) { etat_en_cours_ = AR_EX_VU; } else if (etat_en_cours_ == AR_EX_VU) { etat_en_cours_ = AR_EX_PASSE; attente_ = 200; } if (etat_en_cours_ == AR_DEP) { // dans le cas d'une descente, il se peut qu'il n'y ai // pas de balle etat_en_cours_ = AR_DEP_VU; attente_ = 400; } else if (etat_en_cours_ == AR_DEP_VU) { etat_en_cours_ = AR_DEP_PASSE; attente_ = 400; } } /* interruption fourche optique barillet 1 */ SIGNAL ( SIG_INTERRUPT5 ) { pos_bar(); } /* interruption fourche optique barillet 1 */ SIGNAL ( SIG_INTERRUPT4 ) { 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 ) */