Diff: actions_Gr.cpp

Revision:

6:aa800daf5ff9

Parent:

5:bb533bf81ee6

Child:

7:283d66d5c3b9

--- a/actions_Gr.cpp	Fri May 24 19:36:22 2019 +0000
+++ b/actions_Gr.cpp	Sat May 25 00:58:36 2019 +0000
@@ -23,6 +23,8 @@
 #define MASK_FC_GAUCHE  0x04
 #define MASK_CT_GAUCHE  0x08
 
+#define POS_DOIGT_GAUCHE 585
+#define POS_DOIGT_DROIT  389
 
 char status_pompe=0;
 bool flag_ascenseur = 0;
@@ -60,7 +62,7 @@
             pos = getPos(stockage_G,1);
             if(previous_pos != pos){
                 etat = etalonnage;
-                pc.printf("ETALONNAGE\n");
+                //pc.printf("ETALONNAGE\n");
             }
             break;
 
@@ -72,7 +74,7 @@
                 pos = getPos(stockage_G,1) ;
                 positionControl(stockage_G,pos+180,1,BLED_ON,1);
                 etat = tmp_pret;
-                pc.printf("TMP_PRET\n");
+                //pc.printf("TMP_PRET\n");
             }
             break;
         
@@ -81,17 +83,19 @@
             pos = getPos(stockage_G,1) ;
             if(pos> (previous_pos-5)){
                 etat = pret; 
-                pc.printf("PRET\n");         
+                //pc.printf("PRET\n");         
             } 
             break;
             
         case pret : 
             if((status_contact & MASK_CT_GAUCHE)){
                 pos = getPos(stockage_G,1)+550;
+                deverouillage_torque_convoyeurs_gauche();
+                wait_us(500);
                 positionControl(stockage_G,pos,1,BLED_ON,1);
                 previous_pos = pos;
                 etat = pousse;
-                pc.printf("POUSSE\n");
+                //pc.printf("POUSSE\n");
             }
             break;
             
@@ -99,10 +103,12 @@
             pos = getPos(stockage_G,1);
             if(pos>(previous_pos-5) and (status_contact & MASK_CT_GAUCHE)==0){   
                 pos = pos - 550;        
+                deverouillage_torque_convoyeurs_gauche();
+                wait_us(500);
                 positionControl(stockage_G,pos,1,BLED_ON,1);
                 previous_pos = pos;
                 etat = retour;
-                pc.printf("RETOUR\n");
+                //pc.printf("RETOUR\n");
             }
                 
             break;
@@ -110,7 +116,7 @@
             pos = getPos(stockage_G,1) ;
             if(pos> (previous_pos-5)){
                 etat = pret; 
-                pc.printf("PRET\n");         
+                //pc.printf("PRET\n");         
             } 
             break;
     }   
@@ -131,20 +137,20 @@
             pos = getPos(stockage_D,3);
             if(previous_pos != pos){
                 etat = etalonnage;
-                pc.printf("ETALONNAGE\n");
+                //pc.printf("ETALONNAGE\n");
             }
             break;
 
         case etalonnage :
             pos = getPos(stockage_D,3);
-            if(pos>900 and (status_contact & MASK_FC_DROIT)){ 
+            if(pos>600 and (status_contact & MASK_FC_DROIT)){ 
                 velocityControl(stockage_D,0,GLED_ON,3);
                 wait_ms(500);
                 pos = getPos(stockage_D,3) ;
                 //positionControl(stockage_D,pos-2000,1,BLED_ON,3);
                 compteTour(stockage_D,-1023,2,(pos-500),BLED_ON,3);
                 etat = tmp_pret;
-                pc.printf("TMP_PRET\n");
+                //pc.printf("TMP_PRET\n");
             }
             break;
         
@@ -153,7 +159,7 @@
             pos = getPos(stockage_D,3) ;
             if(pos> (previous_pos+5)){
                 etat = pret; 
-                pc.printf("PRET\n");         
+                //pc.printf("PRET\n");         
             } 
             break;
             
@@ -161,10 +167,12 @@
             if((status_contact & MASK_CT_DROIT)){
                 //pc.printf("%d\n",pos);
                 pos = getPos(stockage_D,3)+ 550;
+                deverouillage_torque_convoyeurs_droit();
+                wait_us(500);
                 positionControl(stockage_D,pos,1,BLED_ON,3);
                 previous_pos = pos;
                 etat = pousse;
-                pc.printf("POUSSE\n");
+                //pc.printf("POUSSE\n");
             }
             break;
             
@@ -173,10 +181,12 @@
             if(pos>(previous_pos-5) and (status_contact & MASK_CT_DROIT)==0){   
                 //pc.printf("%d\n",pos);
                 pos = pos - 550;        
+                deverouillage_torque_convoyeurs_droit();
+                wait_us(500);
                 positionControl(stockage_D,pos,1,BLED_ON,3);
                 previous_pos = pos;
                 etat = retour;
-                pc.printf("RETOUR\n");
+                //pc.printf("RETOUR\n");
             }
                 
             break;
@@ -185,7 +195,7 @@
             pos = getPos(stockage_D,3) ;
             if(pos< (previous_pos+5)){
                 etat = pret; 
-                pc.printf("PRET\n");         
+                //pc.printf("PRET\n");         
             } 
             break;
     }   
@@ -203,6 +213,8 @@
 
     deverouillage_torque();
     positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2);
+    wait(0.5);
+    positionControl(doigt,POS_DOIGT_DROIT,100,BLED_ON,2);
 
 }
 
@@ -232,13 +244,13 @@
                 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;
             FIFO_couleur_ecriture=(FIFO_couleur_ecriture+1)%SIZE_FIFO;
             etat = tmp;
-            break;
+            break;*/
 
         case tmp : //on attend que l'atome traité soit totalement passé
             if(!etat_cap) {
@@ -263,33 +275,25 @@
         case init :
             //on attend le premier atome et place le herkulex en fonction
             if(flag_ascenseur) {
-                SendCharCan(HACHEUR_ID_COUROIES,1);
+                //SendCharCan(HACHEUR_ID_COUROIES,1);
                 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(etat_cap != 0)
+            {
                 oriente_doigt(buffer_couleur_bas[FIFO_couleur_lecture]);
-                /*
-                if(cote)
-                if(buffer_couleur_bas[FIFO_couleur_lecture] == 1) {//position herkulex stockage bleu
-                        positionControl(doigt,640,10,BLED_ON,2);
-                } else if (buffer_couleur_bas[FIFO_couleur_lecture] == 2) {//position herkulex stockage rouge/vert
-                        positionControl(doigt,384,10,GLED_ON,2);
-                }
-                }else{
-                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;
+                etat = tmp;
+            }
+            
+            /*if(buffer_couleur_bas[FIFO_couleur_lecture] != 0) {
+                oriente_doigt(buffer_couleur_bas[FIFO_couleur_lecture]);
+                etat = tmp;
             }else if(etat_cap !=  0) {
                 etat = tmp;
-            }
+            }*/
             break;
 
         case tmp :
@@ -306,15 +310,20 @@
 {
     if(cote) {
         if(palet == 1) {//position herkulex stockage bleu
-            positionControl(doigt,640,10,BLED_ON,2);
+            positionControl(doigt,POS_DOIGT_GAUCHE,10,BLED_ON,2);
+            SendCharCan(HACHEUR_ID_COUROIES,2);
         } else if (palet == 2) {//position herkulex stockage rouge/vert
-            positionControl(doigt,384,10,GLED_ON,2);
+            positionControl(doigt,POS_DOIGT_DROIT,10,GLED_ON,2);
+            SendCharCan(HACHEUR_ID_COUROIES,1);
+            
         }
     } else {
         if(palet== 1) {//position herkulex stockage bleu
-            positionControl(doigt,384,10,BLED_ON,2);
+            positionControl(doigt,POS_DOIGT_DROIT,10,BLED_ON,2);
+            SendCharCan(HACHEUR_ID_COUROIES,1);
         } else if (palet== 2) {//position herkulex stockage rouge/vert
-            positionControl(doigt,640,10,GLED_ON,2);
+            positionControl(doigt,POS_DOIGT_GAUCHE,10,GLED_ON,2);
+            SendCharCan(HACHEUR_ID_COUROIES,2);
         }
     }
 }