Diff: actions_Gr.cpp

Revision:

8:7bd34e838ca3

Parent:

7:283d66d5c3b9

Child:

9:05da4300730a

--- a/actions_Gr.cpp	Sat May 25 15:38:34 2019 +0000
+++ b/actions_Gr.cpp	Sat May 25 16:35:36 2019 +0000
@@ -201,6 +201,84 @@
     }   
 }
 
+void convoyeur_gauche_violet(void){
+    
+   typedef enum {init, etalonnage, tmp_pret, pret, pousse, retour} type_etat ;
+    static type_etat etat = init;
+    static int16_t pos = 0;
+    static int16_t previous_pos = getPos(stockage_G,1);
+    
+    
+    switch(etat){
+            
+        case init : 
+            velocityControl(stockage_G,-512,BLED_ON,1);
+            pos = getPos(stockage_G,1);
+            if(previous_pos != pos){
+                etat = etalonnage;
+                pc.printf("ETALONNAGE\n");
+            }
+            break;
+
+        case etalonnage :
+            pos = getPos(stockage_G,1);
+            if(pos<100 and (status_contact & MASK_FC_GAUCHE)){ 
+                velocityControl(stockage_G,0,GLED_ON,1);
+                wait_ms(500);
+                pos = getPos(stockage_G,1) ;
+                compteTour(stockage_G,1023,2,(pos+500),BLED_ON,1);
+                previous_pos = pos;
+                etat = tmp_pret;
+                pc.printf("TMP_PRET\n");
+            }
+            break;
+        
+        case tmp_pret :
+            pos = getPos(stockage_G,1) ;
+            if(pos> (previous_pos-5)){
+                etat = pret; 
+                pc.printf("PRET\n");         
+            } 
+            break;
+            
+        case pret : 
+            if((status_contact & MASK_CT_GAUCHE)){
+                //pc.printf("%d\n",pos);
+                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");
+            }
+            break;
+            
+        case pousse :
+            pos = getPos(stockage_G,1);
+            if(pos<(previous_pos+5) and (status_contact & MASK_CT_GAUCHE)==0){   
+                //pc.printf("%d\n",pos);
+                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");
+            }
+                
+            break;
+        
+        case retour :
+            pos = getPos(stockage_G,1) ;
+            if(pos> (previous_pos-5)){
+                etat = pret; 
+                pc.printf("PRET\n");         
+            } 
+            break;
+    }   
+}
+
 
 void gabarit_robot(void)
 {