Diff: actions_Gr.cpp

Revision:

17:5973f13f44e2

Parent:

16:06463f24811d

Child:

18:2082875295df

--- a/actions_Gr.cpp	Tue May 28 18:02:59 2019 +0000
+++ b/actions_Gr.cpp	Wed May 29 17:18:16 2019 +0000
@@ -29,7 +29,7 @@
 #define POS_DOIGT_DROIT_SUIV  439
 
 char status_pompe=0;
-bool flag_ascenseur = 0, flag_ascenseur_commande = 0;
+bool flag_ascenseur = 0, flag_ascenseur_force_on = 0, flag_ascenseur_force_off = 0;
 
 char fpresentoir_avant=0, fpresentoir_arriere=0;
 char fgoldenium_avant=0, fgoldenium_arriere=0;
@@ -60,10 +60,12 @@
     if(cote)
     {
         PWM_roue_Gauche.write(float(allume));// = 1.0;//VIT_ROUE*allume;
+        PWM_roue_Droite.write(0);
     }
     else
     {
         PWM_roue_Droite.write(float(allume));// = 1.0;//VIT_ROUE*allume;
+        PWM_roue_Gauche.write(0);
     }
 }
 
@@ -125,6 +127,7 @@
                 flag_vide_vert_rouge =0;
                 flag_debut = 0;
                 etat = vide;
+                SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
             } else if((status_contact & MASK_CT_GAUCHE)) {
                 consigne_pos = (getPos(stockage_G,1)+550);
 
@@ -233,6 +236,9 @@
             
             }else{
                 etat = init; 
+                SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
+                SendCharCan(NB_PALETS_VERTS, nbPaletsVerts);    
+                nbPaletsVerts =0;
                 cpt_vider = 0;
                roue(0);
                 //pc.printf("INIT\n");   
@@ -274,6 +280,7 @@
     static type_etat etat = init;
     static int16_t pos = 0, consigne_pos;
     static int16_t previous_pos = getPos(stockage_D,3);
+    static int flag_ack = 0;
     int vInit = 512;
     /*
     if(flag_vide_bleu){
@@ -319,6 +326,12 @@
             if(pos < (consigne_pos+5)) {
 
                 //pc.printf("pos fin %d\n", pos);
+                if(flag_ack){
+                    SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
+                    SendCharCan(NB_PALETS_BLEU, nbPaletsBleus); 
+                    nbPaletsBleus = 0;   
+                }
+                flag_ack = 1;
                 etat = pret;
                 //pc.printf("PRET\n");
             }
@@ -330,6 +343,7 @@
                 flag_vide_bleu =0;
                 vInit = 1023;
                 etat = init;
+                SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
                 //pc.printf("vide\n");
             } else if((status_contact & MASK_CT_DROIT)) {
                 ///pc.printf("%d\n",pos);
@@ -367,7 +381,7 @@
                 //pc.printf("PRET\n");
             }
             break;
-
+/*
         case vide:
             consigne_pos = getPos(stockage_D,3);
             compteTour(stockage_D,+1023,2,consigne_pos,BLED_ON,3);
@@ -378,10 +392,11 @@
             wait_us(500);
             if(pos> (consigne_pos-5)) {
                 etat = init;
+                SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
                 //pc.printf("INIT\n");
             }
 
-            break;
+            break;*/
     }
 }
 
@@ -393,6 +408,7 @@
     static int16_t pos = 0, consigne_pos, pos_pret;
     static int16_t previous_pos = getPos(stockage_G,1);
     static int vInit = -512;
+    static int flag_ack = 0;
     /*
     if(flag_vide_bleu){
         etat = init;
@@ -435,6 +451,12 @@
             wait_us(500);
             if(pos> (pos_pret-5)) {
                 etat = pret;
+                if(flag_ack){
+                    SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
+                    SendCharCan(NB_PALETS_BLEU, nbPaletsBleus);   
+                    nbPaletsBleus = 0; 
+                }
+                flag_ack = 1;
                 //pc.printf("PRET\n");
             }
             break;
@@ -444,6 +466,7 @@
                 flag_vide_bleu =0;
                 vInit = -1023;
                 etat = init;
+                SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
             } else if((status_contact & MASK_CT_GAUCHE)) {
                 consigne_pos = (pos_pret- 550);
                 if(consigne_pos<0)consigne_pos+=1105;
@@ -485,7 +508,7 @@
                 //pc.printf("PRET\n");
             }
             break;
-
+/*
         case vide:
             consigne_pos = getPos(stockage_G,1);
             compteTour(stockage_G,-1023,2,consigne_pos,BLED_ON,1);
@@ -496,10 +519,12 @@
             wait_us(500);
             if(pos< (consigne_pos+5)) {
                 etat = init;
+                SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
+                SendChar(NB_PALETS_BLEU, nbPaletsVerts);    
                 //pc.printf("PRET\n");
             }
 
-            break;
+            break;*/
     }
 }
 
@@ -560,6 +585,7 @@
             if(flag_vide_vert_rouge) {
                 flag_vide_vert_rouge =0;
                 etat = vide;
+                SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
             } else if((status_contact & MASK_CT_DROIT)) {
                 consigne_pos = getPos(stockage_D,3) - 550;
                 wait_us(500);
@@ -627,6 +653,9 @@
             
             }else{
                 etat = init; 
+                SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
+                SendCharCan(NB_PALETS_VERTS, nbPaletsVerts);    
+                nbPaletsVerts =0;
                 cpt_vider = 0;
                roue(0);
                 //pc.printf("INIT\n");   
@@ -656,7 +685,6 @@
                 cpt_vider ++;
                 etat = vide;
             }
-
             break;
     }
 }
@@ -683,7 +711,7 @@
     positionControl_Mul_ensemble_complex(1,speed,servos_roulette_gauche,pos_roulette_gauche,1);
     
     SendRawId(HACHEUR_ETAT_CONTACTS);
-
+    
 }
 
 
@@ -696,7 +724,7 @@
     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) {
+            if(etat_cap && !flag_ascenseur_force_off) {
                 buffer_couleur_bas[FIFO_couleur_ecriture] = etat_cap; //1 = bleu, 2 = rouge/ vert
                 flag_ascenseur = 1;
                 SendCharCan(HACHEUR_ID_COUROIES,1);
@@ -730,11 +758,13 @@
     static long  cpt;
     static char memo_FIFO_couleur_lecture;
     
+    if(flag_ascenseur_force_off)etat = init;
+    
     switch(etat) {
         case init :
             //on attend le premier atome et place le herkulex en fonction
-            if(flag_ascenseur || flag_ascenseur_commande) {
-                //SendCharCan(HACHEUR_ID_COUROIES,1);
+            if( (flag_ascenseur || flag_ascenseur_force_on) && !flag_ascenseur_force_off ) {
+                SendCharCan(HACHEUR_ID_COUROIES,1);
                 etat = atome;
             }
             break;
@@ -758,7 +788,7 @@
             }else if((cpt > 5000)&& flag_suivant){
                 flag_suivant =0;
                 oriente_doigt_suiv(buffer_couleur_bas[memo_FIFO_couleur_lecture]);
-            }else if( (cpt>60000) && !flag_ascenseur && !flag_ascenseur_commande){
+            }else if( (cpt>60000) && !flag_ascenseur && !flag_ascenseur_force_on){
                 SendCharCan(HACHEUR_ID_COUROIES,0);
                 etat = init;
             }