summaryrefslogtreecommitdiff
path: root/n/es-2007/src/barillet.c
diff options
context:
space:
mode:
Diffstat (limited to 'n/es-2007/src/barillet.c')
-rw-r--r--n/es-2007/src/barillet.c622
1 files changed, 622 insertions, 0 deletions
diff --git a/n/es-2007/src/barillet.c b/n/es-2007/src/barillet.c
new file mode 100644
index 0000000..86e8da3
--- /dev/null
+++ b/n/es-2007/src/barillet.c
@@ -0,0 +1,622 @@
+/* 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 */
+/*************
+ * ATTENTION * cette fonction peut etre appele a tout moment
+ * ***********/
+void
+vidange_barillet(void)
+{
+ OCR_TURB_ARRIERE = VITESSE_TURB_MIN_;
+ OCR_TURB_AVANT = VITESSE_TURB_MIN_;
+ compt_vidange = 0;
+ etat_en_cours_ = VIDANGE_1;
+ rotation_barillet( 0x26, VIDANGE_1 );
+}
+
+
+void sequenceur_barillet()
+{
+ /* DEBUG *
+ 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 ) */