Diff: actions_Gr.cpp

Revision:

9:05da4300730a

Parent:

8:7bd34e838ca3

Child:

10:fba0699b7a74

--- a/actions_Gr.cpp	Sat May 25 16:35:36 2019 +0000
+++ b/actions_Gr.cpp	Sat May 25 17:47:38 2019 +0000
@@ -222,7 +222,8 @@
 
         case etalonnage :
             pos = getPos(stockage_G,1);
-            if(pos<100 and (status_contact & MASK_FC_GAUCHE)){ 
+            pc.printf("%d\n",pos);
+            if((pos>150 and pos<300) and (status_contact & MASK_FC_GAUCHE)){ 
                 velocityControl(stockage_G,0,GLED_ON,1);
                 wait_ms(500);
                 pos = getPos(stockage_G,1) ;
@@ -279,7 +280,81 @@
     }   
 }
 
+void convoyeur_droit_violet(void){
+    
+   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 previous_pos = getPos(stockage_D,3);
+    
+    
+    switch(etat){
+            
+        case init : 
+            velocityControl(stockage_D,+512,BLED_ON,3);
+            pos = getPos(stockage_D,3);
+            if(previous_pos != pos){
+                pc.printf("ETALONNAGE\n");
+                etat = etalonnage;
+            }
+            break;
 
+        case etalonnage :
+            pos = getPos(stockage_D,3);
+            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) ;
+                positionControl(stockage_D,pos-180,1,BLED_ON,3);
+                previous_pos = pos;
+                etat = tmp_pret;
+                pc.printf("TMP_PRET\n");
+            }
+            break;
+        
+        case tmp_pret :  
+            pos = getPos(stockage_D,3) ;
+            if(pos< (previous_pos+5)){
+                etat = pret; 
+                pc.printf("PRET\n");         
+            } 
+            break;
+            
+        case pret : 
+            if((status_contact & MASK_CT_DROIT)){
+                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");
+            }
+            break;
+            
+        case pousse :
+            pos = getPos(stockage_D,3);
+            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);
+                previous_pos = pos;
+                etat = retour;
+                pc.printf("RETOUR\n");
+            }
+                
+            break;
+        case retour :
+            pos = getPos(stockage_D,3) ;
+            if(pos> (previous_pos-5)){
+                etat = pret; 
+                pc.printf("PRET\n");         
+            } 
+            break;
+    }   
+} 
 void gabarit_robot(void)
 {
     uint8_t servos_av_centre[4] = {GLED_ON, AV_EP_C, GLED_ON, AV_poigne_C};