Diff: actions_Gr.cpp

Revision:

2:33583329d6c8

Parent:

1:af508f84a079

Child:

3:a630a1ccf5f0

diff -r af508f84a079 -r 33583329d6c8 actions_Gr.cpp
--- a/actions_Gr.cpp	Thu May 23 11:44:30 2019 +0000
+++ b/actions_Gr.cpp	Thu May 23 14:15:46 2019 +0000
@@ -171,16 +171,75 @@
 
 void presentoir_avant(void)
 {
+    uint8_t servos_av_centre[4] = {GLED_ON, AV_EP_C, GLED_ON, AV_poigne_C};
+
+    uint16_t pos_av_centre[2] = {470,512};
+
+    int speed=1;
+
+    positionControl_Mul_ensemble_complex(2,speed,servos_av_centre, pos_av_centre,2);
+    verification();
 }
 
 
 void automate_ventouse_presentoir_avant(void)
 {
+    typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
+    static type_etat etat = init;
+
+    switch(etat) {
+        case init: //attente d'initialisation
+            if(fpresentoir_avant)
+                etat=envoi_instruction;
+            break;
+
+        case envoi_instruction://envoi instruction
+            SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
+            presentoir_avant();
+            SendRawId(HACHEUR_GET_PRESENTOIR_AV);
+            SendRawId(HACHEUR_STATUT_VENTOUSES);
+            etat = attente_ack_ventouse;
+            break;
+
+        case attente_ack_ventouse:
+            if((status_pompe&MASK_PRESENTOIR_AV)==MASK_PRESENTOIR_AV) {
+                fpresentoir_avant=0;
+                SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
+                etat = init;
+            }
+            break;
+
+    }
 }
 
 void automate_ventouse_relache_avant(void)
 {
 
+    typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
+    static type_etat etat = init;
+
+    switch(etat) {
+        case init: //attente d'initialisation
+            if(favant_relache)
+                etat=envoi_instruction;
+            break;
+
+        case envoi_instruction://envoi instruction
+            SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
+            SendRawId(HACHEUR_STATUT_VENTOUSES);
+            etat = attente_ack_ventouse;
+            break;
+
+        case attente_ack_ventouse:
+            SendRawId(HACHEUR_RELEASE_AV);
+            if((status_pompe&MASK_PRESENTOIR_AV)== 0) {
+                favant_relache=0;
+                SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
+                etat = init;
+            }
+            break;
+
+    }
 
 }