Diff: actions_Gr.cpp

Revision:

10:fba0699b7a74

Parent:

9:05da4300730a

Child:

11:dfff2e200d0c

--- a/actions_Gr.cpp	Sat May 25 17:47:38 2019 +0000
+++ b/actions_Gr.cpp	Sat May 25 18:39:49 2019 +0000
@@ -62,7 +62,7 @@
             pos = getPos(stockage_G,1);
             if(previous_pos != pos){
                 etat = etalonnage;
-                //pc.printf("ETALONNAGE\n");
+                pc.printf("ETALONNAGE\n");
             }
             break;
 
@@ -74,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;
         
@@ -83,7 +83,7 @@
             pos = getPos(stockage_G,1) ;
             if(pos> (previous_pos-5)){
                 etat = pret; 
-                //pc.printf("PRET\n");         
+                pc.printf("PRET\n");         
             } 
             break;
             
@@ -95,7 +95,7 @@
                 positionControl(stockage_G,pos,1,BLED_ON,1);
                 previous_pos = pos;
                 etat = pousse;
-                //pc.printf("POUSSE\n");
+                pc.printf("POUSSE\n");
             }
             break;
             
@@ -108,7 +108,7 @@
                 positionControl(stockage_G,pos,1,BLED_ON,1);
                 previous_pos = pos;
                 etat = retour;
-                //pc.printf("RETOUR\n");
+                pc.printf("RETOUR\n");
             }
                 
             break;
@@ -116,7 +116,7 @@
             pos = getPos(stockage_G,1) ;
             if(pos> (previous_pos-5)){
                 etat = pret; 
-                //pc.printf("PRET\n");         
+                pc.printf("PRET\n");         
             } 
             break;
     }   
@@ -134,6 +134,7 @@
             
         case init : 
             velocityControl(stockage_D,512,BLED_ON,3);
+                wait_us(500);
             pos = getPos(stockage_D,3);
             if(previous_pos != pos){
                 etat = etalonnage;
@@ -149,6 +150,7 @@
                 pos = getPos(stockage_D,3) ;
                 //positionControl(stockage_D,pos-2000,1,BLED_ON,3);
                 compteTour(stockage_D,-1023,2,(pos-500),BLED_ON,3);
+                wait_us(500);
                 etat = tmp_pret;
                 //pc.printf("TMP_PRET\n");
             }
@@ -157,6 +159,7 @@
         case tmp_pret :
             previous_pos = pos;
             pos = getPos(stockage_D,3) ;
+                wait_us(500);
             if(pos> (previous_pos+5)){
                 etat = pret; 
                 //pc.printf("PRET\n");         
@@ -170,6 +173,7 @@
                 deverouillage_torque_convoyeurs_droit();
                 wait_us(500);
                 positionControl(stockage_D,pos,1,BLED_ON,3);
+                wait_us(500);
                 previous_pos = pos;
                 etat = pousse;
                 //pc.printf("POUSSE\n");
@@ -178,12 +182,14 @@
             
         case pousse :
             pos = getPos(stockage_D,3);
+                wait_us(500);
             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);
+                wait_us(500);
                 previous_pos = pos;
                 etat = retour;
                 //pc.printf("RETOUR\n");
@@ -193,6 +199,7 @@
         
         case retour :
             pos = getPos(stockage_D,3) ;
+                wait_us(500);
             if(pos< (previous_pos+5)){
                 etat = pret; 
                 //pc.printf("PRET\n");         
@@ -213,10 +220,12 @@
             
         case init : 
             velocityControl(stockage_G,-512,BLED_ON,1);
+                wait_us(500);
             pos = getPos(stockage_G,1);
+                wait_us(500);
             if(previous_pos != pos){
                 etat = etalonnage;
-                pc.printf("ETALONNAGE\n");
+               // pc.printf("ETALONNAGE\n");
             }
             break;
 
@@ -227,15 +236,18 @@
                 velocityControl(stockage_G,0,GLED_ON,1);
                 wait_ms(500);
                 pos = getPos(stockage_G,1) ;
+                wait_us(500);
                 compteTour(stockage_G,1023,2,(pos+500),BLED_ON,1);
+                wait_us(500);
                 previous_pos = pos;
                 etat = tmp_pret;
-                pc.printf("TMP_PRET\n");
+                ////pc.printf("TMP_PRET\n");
             }
             break;
         
         case tmp_pret :
             pos = getPos(stockage_G,1) ;
+                wait_us(500);
             if(pos> (previous_pos-5)){
                 etat = pret; 
                 pc.printf("PRET\n");         
@@ -246,32 +258,37 @@
             if((status_contact & MASK_CT_GAUCHE)){
                 //pc.printf("%d\n",pos);
                 pos = getPos(stockage_G,1)- 550;
+                wait_us(500);
                 deverouillage_torque_convoyeurs_gauche();
                 wait_us(500);
                 positionControl(stockage_G,pos,1,BLED_ON,1);
+                wait_us(500);
                 previous_pos = pos;
                 etat = pousse;
-                pc.printf("POUSSE\n");
+                //pc.printf("POUSSE\n");
             }
             break;
             
         case pousse :
             pos = getPos(stockage_G,1);
+                wait_us(500);
             if(pos<(previous_pos+5) and (status_contact & MASK_CT_GAUCHE)==0){   
-                //pc.printf("%d\n",pos);
+                pc.printf("%d\n",pos);
                 pos = pos + 550;        
                 deverouillage_torque_convoyeurs_gauche();
                 wait_us(500);
                 positionControl(stockage_G,pos,1,BLED_ON,1);
+                wait_us(500);
                 previous_pos = pos;
                 etat = retour;
-                pc.printf("RETOUR\n");
+                //pc.printf("RETOUR\n");
             }
                 
             break;
         
         case retour :
             pos = getPos(stockage_G,1) ;
+                wait_us(500);
             if(pos> (previous_pos-5)){
                 etat = pret; 
                 pc.printf("PRET\n");         
@@ -292,65 +309,76 @@
             
         case init : 
             velocityControl(stockage_D,+512,BLED_ON,3);
+                wait_us(500);
             pos = getPos(stockage_D,3);
+                wait_us(500);
             if(previous_pos != pos){
-                pc.printf("ETALONNAGE\n");
+                //pc.printf("ETALONNAGE\n");
                 etat = etalonnage;
             }
             break;
 
         case etalonnage :
             pos = getPos(stockage_D,3);
-            pc.printf("%d\n", pos);
+                wait_us(500);
+            //pc.printf("%d\n", pos);
             if(pos>900 and (status_contact & MASK_FC_DROIT)){ 
                 velocityControl(stockage_D,0,GLED_ON,3);
                 wait_ms(500);
                 pos = getPos(stockage_D,3) ;
+                wait_us(500);
                 positionControl(stockage_D,pos-180,1,BLED_ON,3);
+                wait_us(500);
                 previous_pos = pos;
                 etat = tmp_pret;
-                pc.printf("TMP_PRET\n");
+               // pc.printf("TMP_PRET\n");
             }
             break;
         
         case tmp_pret :  
             pos = getPos(stockage_D,3) ;
+                wait_us(500);
             if(pos< (previous_pos+5)){
                 etat = pret; 
-                pc.printf("PRET\n");         
+                //pc.printf("PRET\n");         
             } 
             break;
             
         case pret : 
             if((status_contact & MASK_CT_DROIT)){
                 pos = getPos(stockage_D,3)- 550;
+                wait_us(500);
                 deverouillage_torque_convoyeurs_droit();
                 wait_us(500);
                 positionControl(stockage_D,pos,1,BLED_ON,3);
+                wait_us(500);
                 previous_pos = pos;
                 etat = pousse;
-                pc.printf("POUSSE\n");
+                //pc.printf("POUSSE\n");
             }
             break;
             
         case pousse :
             pos = getPos(stockage_D,3);
+                wait_us(500);
             if(pos<(previous_pos+5) and (status_contact & MASK_CT_DROIT)==0){   
                 pos = pos + 550;        
                 deverouillage_torque_convoyeurs_droit();
                 wait_us(500);
                 positionControl(stockage_D,pos,1,BLED_ON,3);
+                wait_us(500);
                 previous_pos = pos;
                 etat = retour;
-                pc.printf("RETOUR\n");
+                //pc.printf("RETOUR\n");
             }
                 
             break;
         case retour :
             pos = getPos(stockage_D,3) ;
+                wait_us(500);
             if(pos> (previous_pos-5)){
                 etat = pret; 
-                pc.printf("PRET\n");         
+                //pc.printf("PRET\n");         
             } 
             break;
     }   
@@ -367,7 +395,7 @@
     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);
+    
 
 }
 
@@ -385,11 +413,6 @@
                 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
-                    }*/
                     oriente_doigt(buffer_couleur_bas[FIFO_couleur_ecriture]);
                 }
                 
@@ -397,13 +420,6 @@
                 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;*/
 
         case tmp : //on attend que l'atome traité soit totalement passé
             if(!etat_cap) {
@@ -440,19 +456,25 @@
                 oriente_doigt(etat_cap);
                 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;
+            /*else if(FIFO_couleur_lecture == FIFO_couleur_ecriture)
+            {
+                oriente_doigt((FIFO_couleur_lecture-1)%SIZE_FIFO);
             }*/
             break;
 
         case tmp :
             //on attend que le capteur soit totalement passé pour déplacer le pointeur de lecture
             if(etat_cap == 0) {
+                char memo_FIFO_couleur_lecture = FIFO_couleur_lecture;
                 FIFO_couleur_lecture=(FIFO_couleur_lecture+1)%SIZE_FIFO;
+                if(FIFO_couleur_lecture == FIFO_couleur_ecriture)
+                {
+                    pc.printf("Fifo vide\n");
+                    if(buffer_couleur_bas[memo_FIFO_couleur_lecture] == 1)
+                        oriente_doigt(2);
+                    else if(buffer_couleur_bas[memo_FIFO_couleur_lecture] == 2)
+                        oriente_doigt(1);
+                }
                 etat = atome;
             }
             break;