Diff: actions_Gr.cpp

Revision:

0:f900f4ea9dfd

Child:

1:af508f84a079

diff -r 000000000000 -r f900f4ea9dfd actions_Gr.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/actions_Gr.cpp	Thu May 23 10:47:57 2019 +0000
@@ -0,0 +1,204 @@
+#include "actions_Gr.h"
+
+
+#ifdef ROBOT_BIG
+#define MASK_PRESENTOIR_AV 0x07
+#define MASK_PRESENTOIR_AR 0x70
+
+#define MASK_GOLDENIUM_AV 0x02
+#define MASK_GOLDENIUM_AR 0x20
+
+#define MASK_SOL_AV 0x08
+#define MASK_SOL_AR 0x80
+
+#define MASK_AV_DROIT 0x01
+#define MASK_AR_DROIT 0x10
+
+#define MASK_AV_DROIT_GAUCHE 0x05
+#define MASK_AR_DROIT_GAUCHE 0x50
+
+
+char status_pompe=0;
+bool flag_ascenseur = 0;
+
+char fpresentoir_avant=0, fpresentoir_arriere=0;
+char fgoldenium_avant=0, fgoldenium_arriere=0;
+char fsol_avant=0,fsol_arriere=0;
+char fsol_avant_relache=0,fsol_arriere_relache=0;
+char fbalance_avant=0,fbalance_arriere=0;
+char favant_relache=0,farriere_relache=0;
+char faccelerateur_avant=0,faccelerateur_arriere=0;
+
+
+DigitalIn couleur_haut[3] = {PA_9,PA_10,PA_11}; //GC1
+DigitalIn couleur_bas[3] = {PB_12,PB_13,PB_14}; //GC2
+DigitalIn presence_droit(PB_5);
+DigitalIn presence_gauche(PB_4);
+
+
+char buffer_couleur_bas[SIZE_FIFO];
+unsigned char FIFO_couleur_ecriture=0;
+signed char FIFO_couleur_lecture=0;
+
+
+
+
+
+void gabarit_gros_robot(void)
+{
+    uint8_t servos_av_centre[4] = {GLED_ON, AV_EP_C, GLED_ON, AV_poigne_C};
+
+    uint16_t pos_av_centre[2] = {550,600};//470,350
+
+
+    int speed=100;
+
+    deverouillage_torque();
+    //positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2);
+
+}
+
+
+void fifo_couleur(void)
+{
+
+    typedef enum {n_atome, atome, tmp} type_etat ;
+    static type_etat etat = n_atome;
+
+    int etat_cap = !couleur_bas[0] + !couleur_bas[1]*2 + !couleur_bas[2]*2;
+    switch(etat) {
+        case n_atome : //on attend qu'un atome soit sous le capteur pour lancer la FIFO
+            if(etat_cap) {
+                buffer_couleur_bas[FIFO_couleur_ecriture] = etat_cap; //1 = bleu, 2 = rouge/ vert
+                flag_ascenseur = 1;
+                if(FIFO_couleur_ecriture == FIFO_couleur_lecture) {
+                    if(buffer_couleur_bas[FIFO_couleur_ecriture] == 1) {
+                        positionControl(doigt,384,10,BLED_ON,2);   //position herkulex stockage bleu
+                    } else {
+                        positionControl(doigt,640,10,GLED_ON,2);   //position herkulex stockage rouge/vert
+                    }
+                }
+                //pc.printf("fifo ecriture %d\n", buffer_couleur_bas[FIFO_couleur_ecriture]);
+                FIFO_couleur_ecriture=(FIFO_couleur_ecriture+1)%SIZE_FIFO;
+                etat = tmp;
+            }
+            break;
+
+        case atome : //on stocke dans la fifo la couleur de l'atome
+            buffer_couleur_bas[FIFO_couleur_ecriture] = etat_cap; //1 = bleu, 2 = rouge/ vert
+            flag_ascenseur = 1;
+            /*if(buffer_couleur_bas[FIFO_couleur_ecriture] == 1) {
+                    positionControl(doigt,384,1,BLED_ON,2);   //position herkulex stockage bleu
+                } else {
+                    positionControl(doigt,640,1,GLED_ON,2);   //position herkulex stockage rouge/vert
+                }*/
+            //pc.printf("fifo ecriture %d\n", buffer_couleur_bas[FIFO_couleur_ecriture]);
+            FIFO_couleur_ecriture=(FIFO_couleur_ecriture+1)%SIZE_FIFO;
+            etat = tmp;
+            break;
+
+        case tmp : //on attend que l'atome traité soit totalement passé
+            if(!etat_cap) {
+                etat = n_atome;
+            }
+            break;
+
+    }
+
+}
+
+
+void ascenseur(void)
+{
+    typedef enum {init, atome, tmp} type_etat;
+    static type_etat etat = init;
+
+    int etat_cap = !couleur_haut[0] + !couleur_haut[1]*2 + !couleur_haut[2]*2;
+    static int flag_sortie = 0;
+    switch(etat) {
+        case init :
+            //on attend le premier atome et place le herkulex en fonction
+            if(flag_ascenseur) {
+                /*if(buffer_couleur_bas[FIFO_couleur_lecture] == 1) {
+                    positionControl(doigt,384,1,BLED_ON,2);   //position herkulex stockage bleu
+                } else {
+                    positionControl(doigt,640,1,GLED_ON,2);   //position herkulex stockage rouge/vert
+                }*/
+                etat = atome;
+            }
+            break;
+
+        case atome :
+            //on attend que l'atome soit présent devant le capteur haut et qu'il corresponde à la FIFO
+            if(buffer_couleur_bas[FIFO_couleur_lecture] != 0) {
+                if(buffer_couleur_bas[FIFO_couleur_lecture] == etat_cap) {
+                    /*if(flag_sortie) {
+                        if(buffer_couleur_bas[FIFO_couleur_lecture] == 1) {//position herkulex stockage bleu
+                            positionControl(doigt,384,10,BLED_ON,2);
+                        } else if (buffer_couleur_bas[FIFO_couleur_lecture] == 2) {//position herkulex stockage rouge/vert
+                            positionControl(doigt,640,10,GLED_ON,2);
+                        }
+                        flag_sortie = 0;
+                    }
+                    pc.printf("--fifo lecture %d\n", buffer_couleur_bas[FIFO_couleur_lecture]);
+                    //on change la position du herkulex si le prochain atome est différent
+                    if(buffer_couleur_bas[FIFO_couleur_lecture] != buffer_couleur_bas[FIFO_couleur_lecture+1] ) {
+                        flag_sortie = 1;
+                    }*/
+                 if(buffer_couleur_bas[FIFO_couleur_lecture] == 1) {//position herkulex stockage bleu
+                        positionControl(doigt,384,10,BLED_ON,2);
+                    } else if (buffer_couleur_bas[FIFO_couleur_lecture] == 2) {//position herkulex stockage rouge/vert
+                        positionControl(doigt,640,10,GLED_ON,2);
+                    }
+                    etat = tmp;
+                } else if(etat_cap !=  0) {
+                    //pc.printf("--fifo lecture %d --- vu : %d\n", buffer_couleur_bas[FIFO_couleur_lecture], etat_cap);
+                    etat = tmp;
+                }
+            }
+            break;
+
+        case tmp :
+            //on attend que le capteur soit totalement passé pour déplacer le pointeur de lecture
+            if(etat_cap == 0) {
+                //pc.printf("\t\tfifo + 1\n");
+                FIFO_couleur_lecture=(FIFO_couleur_lecture+1)%SIZE_FIFO;
+                etat = atome;
+            }
+            break;
+    }
+}
+/*
+void presentoir_avant(void)
+{
+}
+
+
+void automate_ventouse_presentoir_avant(void)
+{
+}
+
+void automate_ventouse_relache_avant(void)
+{
+
+
+}
+
+void goldenium_avant(void)
+{
+
+}
+
+void automate_ventouse_goldenium_avant (void)
+{
+}
+
+void accelerateur_avant(void)
+{
+}
+
+void automate_ventouse_accelerateur_avant(void)
+{
+
+}*/
+#endif
\ No newline at end of file