Diff: actions_Gr.cpp

Revision:

11:dfff2e200d0c

Parent:

10:fba0699b7a74

Child:

13:0b5310c6dc44

--- a/actions_Gr.cpp	Sat May 25 18:39:49 2019 +0000
+++ b/actions_Gr.cpp	Sun May 26 09:58:39 2019 +0000
@@ -51,7 +51,7 @@
     
     typedef enum {init, etalonnage, tmp_pret, pret, pousse, tmp,retour} type_etat ;
     static type_etat etat = init;
-    static int16_t pos = 0;
+    static int16_t pos = 0, consigne_pos;
     static int16_t previous_pos = getPos(stockage_G,1);
     
     
@@ -62,61 +62,65 @@
             pos = getPos(stockage_G,1);
             if(previous_pos != pos){
                 etat = etalonnage;
-                pc.printf("ETALONNAGE\n");
+               //pc.printf("ETALONNAGE\n");
+                
             }
             break;
 
         case etalonnage :
             pos = getPos(stockage_G,1);
-            if(pos<100 and (status_contact & MASK_FC_GAUCHE)){ 
+            wait_us(500);
+            if(   ( (21<pos && pos<260) || (404<pos && pos<812) )  and (status_contact & MASK_FC_GAUCHE)){ 
                 velocityControl(stockage_G,0,GLED_ON,1);
-                wait_ms(500);
+                wait_ms(300);
                 pos = getPos(stockage_G,1) ;
-                positionControl(stockage_G,pos+180,1,BLED_ON,1);
+                consigne_pos = pos + 180;
+                positionControl(stockage_G,consigne_pos,1,BLED_ON,1);
                 etat = tmp_pret;
-                pc.printf("TMP_PRET\n");
+               //pc.printf("TMP_PRET\n");
             }
             break;
         
         case tmp_pret :
-            previous_pos = pos;
-            pos = getPos(stockage_G,1) ;
-            if(pos> (previous_pos-5)){
+            pos = getPos(stockage_G,1);
+            wait_us(500);
+            if(pos> (consigne_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;
+                consigne_pos = getPos(stockage_G,1)+550;
                 deverouillage_torque_convoyeurs_gauche();
                 wait_us(500);
-                positionControl(stockage_G,pos,1,BLED_ON,1);
-                previous_pos = pos;
+                positionControl(stockage_G,consigne_pos,1,BLED_ON,1);
                 etat = pousse;
-                pc.printf("POUSSE\n");
+               //pc.printf("POUSSE\n");
             }
             break;
             
         case pousse :
             pos = getPos(stockage_G,1);
-            if(pos>(previous_pos-5) and (status_contact & MASK_CT_GAUCHE)==0){   
-                pos = pos - 550;        
+            wait_us(500);
+            if(pos>(consigne_pos-5) and (status_contact & MASK_CT_GAUCHE)==0){   
+                consigne_pos = pos - 550;        
                 deverouillage_torque_convoyeurs_gauche();
                 wait_us(500);
-                positionControl(stockage_G,pos,1,BLED_ON,1);
-                previous_pos = pos;
+                positionControl(stockage_G,consigne_pos,1,BLED_ON,1);
                 etat = retour;
-                pc.printf("RETOUR\n");
+               //pc.printf("RETOUR\n");
             }
                 
             break;
         case retour :
             pos = getPos(stockage_G,1) ;
-            if(pos> (previous_pos-5)){
+            wait_us(500);
+            if(pos< (consigne_pos+5)){
                 etat = pret; 
-                pc.printf("PRET\n");         
+                //pc.printf("PRET\n");         
             } 
             break;
     }   
@@ -126,7 +130,7 @@
     
     typedef enum {init, etalonnage, tmp_pret, pret, pousse, retour} type_etat ;
     static type_etat etat = init;
-    static int16_t pos = 0;
+    static int16_t pos = 0, consigne_pos;
     static int16_t previous_pos = getPos(stockage_D,3);
     
     
@@ -134,75 +138,76 @@
             
         case init : 
             velocityControl(stockage_D,512,BLED_ON,3);
-                wait_us(500);
+            wait_us(500);
             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>600 and (status_contact & MASK_FC_DROIT)){ 
+            wait_us(500);
+            if(( (21<pos && pos<428) || (572<pos && pos<980) ) and (status_contact & MASK_FC_DROIT)){ 
                 velocityControl(stockage_D,0,GLED_ON,3);
-                wait_ms(500);
+                wait_ms(300);
                 pos = getPos(stockage_D,3) ;
                 //positionControl(stockage_D,pos-2000,1,BLED_ON,3);
-                compteTour(stockage_D,-1023,2,(pos-500),BLED_ON,3);
+                consigne_pos =pos-500;
+                if(consigne_pos<0) consigne_pos += 1105;
+                pc.printf("pos : %d, consigne : %d \n", pos, consigne_pos);
+                compteTour(stockage_D,-1023,2,consigne_pos,BLED_ON,3);
                 wait_us(500);
                 etat = tmp_pret;
-                //pc.printf("TMP_PRET\n");
+                pc.printf("TMP_PRET\n");
             }
             break;
         
         case tmp_pret :
-            previous_pos = pos;
             pos = getPos(stockage_D,3) ;
                 wait_us(500);
-            if(pos> (previous_pos+5)){
+            if(pos < (consigne_pos+5)){
                 etat = pret; 
-                //pc.printf("PRET\n");         
+                pc.printf("PRET\n");         
             } 
             break;
             
         case pret : 
             if((status_contact & MASK_CT_DROIT)){
                 //pc.printf("%d\n",pos);
-                pos = getPos(stockage_D,3)+ 550;
+                consigne_pos = getPos(stockage_D,3)+ 550;
                 deverouillage_torque_convoyeurs_droit();
                 wait_us(500);
-                positionControl(stockage_D,pos,1,BLED_ON,3);
+                positionControl(stockage_D,consigne_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){   
+            if(pos>(consigne_pos-5) and (status_contact & MASK_CT_DROIT)==0){   
                 //pc.printf("%d\n",pos);
-                pos = pos - 550;        
+                consigne_pos = pos - 550;        
                 deverouillage_torque_convoyeurs_droit();
                 wait_us(500);
-                positionControl(stockage_D,pos,1,BLED_ON,3);
+                positionControl(stockage_D,consigne_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)){
+            wait_us(500);
+            if(pos< (consigne_pos+5)){
                 etat = pret; 
-                //pc.printf("PRET\n");         
+                pc.printf("PRET\n");         
             } 
             break;
     }   
@@ -212,7 +217,7 @@
     
    typedef enum {init, etalonnage, tmp_pret, pret, pousse, retour} type_etat ;
     static type_etat etat = init;
-    static int16_t pos = 0;
+    static int16_t pos = 0, consigne_pos;
     static int16_t previous_pos = getPos(stockage_G,1);
     
     
@@ -232,14 +237,15 @@
         case etalonnage :
             pos = getPos(stockage_G,1);
             pc.printf("%d\n",pos);
-            if((pos>150 and pos<300) and (status_contact & MASK_FC_GAUCHE)){ 
+            if((21<pos && pos<428) || (572<pos && pos<980) and (status_contact & MASK_FC_GAUCHE)){ //!!!!!!!!!!!!!changer les bornes
                 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);
+                consigne_pos = pos + 500;
+                if(consigne_pos >1105) consigne_pos -= 1105;
+                compteTour(stockage_G,1023,2,consigne_pos,BLED_ON,1);
                 wait_us(500);
-                previous_pos = pos;
                 etat = tmp_pret;
                 ////pc.printf("TMP_PRET\n");
             }
@@ -248,7 +254,7 @@
         case tmp_pret :
             pos = getPos(stockage_G,1) ;
                 wait_us(500);
-            if(pos> (previous_pos-5)){
+            if(pos> (consigne_pos-5)){
                 etat = pret; 
                 pc.printf("PRET\n");         
             } 
@@ -257,13 +263,12 @@
         case pret : 
             if((status_contact & MASK_CT_GAUCHE)){
                 //pc.printf("%d\n",pos);
-                pos = getPos(stockage_G,1)- 550;
+                consigne_pos = getPos(stockage_G,1)- 550;
                 wait_us(500);
                 deverouillage_torque_convoyeurs_gauche();
                 wait_us(500);
-                positionControl(stockage_G,pos,1,BLED_ON,1);
+                positionControl(stockage_G,consigne_pos,1,BLED_ON,1);
                 wait_us(500);
-                previous_pos = pos;
                 etat = pousse;
                 //pc.printf("POUSSE\n");
             }
@@ -272,14 +277,13 @@
         case pousse :
             pos = getPos(stockage_G,1);
                 wait_us(500);
-            if(pos<(previous_pos+5) and (status_contact & MASK_CT_GAUCHE)==0){   
+            if(pos<(consigne_pos+5) and (status_contact & MASK_CT_GAUCHE)==0){   
                 pc.printf("%d\n",pos);
-                pos = pos + 550;        
+                consigne_pos = pos + 550;        
                 deverouillage_torque_convoyeurs_gauche();
                 wait_us(500);
-                positionControl(stockage_G,pos,1,BLED_ON,1);
+                positionControl(stockage_G,consigne_pos,1,BLED_ON,1);
                 wait_us(500);
-                previous_pos = pos;
                 etat = retour;
                 //pc.printf("RETOUR\n");
             }
@@ -289,7 +293,7 @@
         case retour :
             pos = getPos(stockage_G,1) ;
                 wait_us(500);
-            if(pos> (previous_pos-5)){
+            if(pos> (consigne_pos-5)){
                 etat = pret; 
                 pc.printf("PRET\n");         
             } 
@@ -301,7 +305,7 @@
     
    typedef enum {init, etalonnage, tmp_pret, pret, pousse, tmp,retour} type_etat ;
     static type_etat etat = init;
-    static int16_t pos = 0;
+    static int16_t pos = 0, consigne_pos;
     static int16_t previous_pos = getPos(stockage_D,3);
     
     
@@ -322,14 +326,13 @@
             pos = getPos(stockage_D,3);
                 wait_us(500);
             //pc.printf("%d\n", pos);
-            if(pos>900 and (status_contact & MASK_FC_DROIT)){ 
+            if(  (21<pos && pos<260) || (404<pos && pos<812) and (status_contact & MASK_FC_DROIT)){ //!!!!!!!!!!!!!changer les bornes
                 velocityControl(stockage_D,0,GLED_ON,3);
                 wait_ms(500);
-                pos = getPos(stockage_D,3) ;
+                consigne_pos = getPos(stockage_D,3) - 180 ;
                 wait_us(500);
-                positionControl(stockage_D,pos-180,1,BLED_ON,3);
+                positionControl(stockage_D,consigne_pos,1,BLED_ON,3);
                 wait_us(500);
-                previous_pos = pos;
                 etat = tmp_pret;
                // pc.printf("TMP_PRET\n");
             }
@@ -338,7 +341,7 @@
         case tmp_pret :  
             pos = getPos(stockage_D,3) ;
                 wait_us(500);
-            if(pos< (previous_pos+5)){
+            if(pos< (consigne_pos+5)){
                 etat = pret; 
                 //pc.printf("PRET\n");         
             } 
@@ -346,13 +349,12 @@
             
         case pret : 
             if((status_contact & MASK_CT_DROIT)){
-                pos = getPos(stockage_D,3)- 550;
+                consigne_pos = getPos(stockage_D,3) - 550;
                 wait_us(500);
                 deverouillage_torque_convoyeurs_droit();
                 wait_us(500);
-                positionControl(stockage_D,pos,1,BLED_ON,3);
+                positionControl(stockage_D,consigne_pos,1,BLED_ON,3);
                 wait_us(500);
-                previous_pos = pos;
                 etat = pousse;
                 //pc.printf("POUSSE\n");
             }
@@ -361,13 +363,12 @@
         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;        
+            if(pos<(consigne_pos+5) and (status_contact & MASK_CT_DROIT)==0){   
+                consigne_pos = pos + 550;        
                 deverouillage_torque_convoyeurs_droit();
                 wait_us(500);
-                positionControl(stockage_D,pos,1,BLED_ON,3);
+                positionControl(stockage_D,consigne_pos,1,BLED_ON,3);
                 wait_us(500);
-                previous_pos = pos;
                 etat = retour;
                 //pc.printf("RETOUR\n");
             }
@@ -376,7 +377,7 @@
         case retour :
             pos = getPos(stockage_D,3) ;
                 wait_us(500);
-            if(pos> (previous_pos-5)){
+            if(pos> (consigne_pos-5)){
                 etat = pret; 
                 //pc.printf("PRET\n");         
             }