Diff: main.cpp

Revision:

11:6c11c081c855

Parent:

10:48c0d1b8aed9

Child:

12:191486ba5118

diff -r 48c0d1b8aed9 -r 6c11c081c855 main.cpp
--- a/main.cpp	Sat May 18 12:07:01 2019 +0000
+++ b/main.cpp	Mon May 20 11:38:43 2019 +0000
@@ -20,7 +20,7 @@
 char  fgoldenium_avant=0, fgoldenium_arriere=0;
 char fsol_avant=0,fsol_arriere=0;
 char fsol_avant_relache=0,fsol_arriere_relache=0;
-
+char fbalance_avant=0;
 char status_pompe=0;
 
 void canRx_ISR (void);
@@ -40,6 +40,8 @@
 
 void automate_ventouse_sol_avant_relache(void);
 void automate_ventouse_sol_arriere_relache(void);
+
+void automate_ventouse_balance_avant (void);
 /*********************************************************************************************/
 /* FUNCTION NAME: canRx_ISR                                                                  */
 /* DESCRIPTION  : lit les messages sur le can et les stocke dans la FIFO                     */
@@ -122,13 +124,13 @@
 {
     can.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN
     servo_interrupt_en(); //permettre les interuptions
+
     wait(1);//attente servo boot
     gabarit_petit_robot();
-
     while(1) {
         canProcessRx();
         f_mesure();//dt35
-        
+
         automate_ventouse_presentoir_avant();
         automate_ventouse_presentoir_arriere();
 
@@ -138,6 +140,8 @@
         automate_ventouse_sol_avant();
         automate_ventouse_sol_arriere();
 
+        automate_ventouse_balance_avant();
+        
         if(EtatGameEnd==1) {
             while(1);
         }
@@ -224,9 +228,7 @@
                 break;
 
             case BALANCE_AVANT:
-                SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
-                balance_avant();
-                SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
+                fbalance_avant=1;
                 break;
 
             case BALANCE_ARRIERE:
@@ -491,4 +493,34 @@
     }
 
 
+}
+void automate_ventouse_balance_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(fbalance_avant)
+                etat=envoi_instruction;
+            break;
+
+        case envoi_instruction://envoi instruction
+            SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
+            balance_avant();
+            SendRawId(HACHEUR_STATUT_VENTOUSES);
+            etat = attente_ack_ventouse;
+            break;
+
+        case attente_ack_ventouse:
+            if((status_pompe&MASK_AV_DROIT)== 0) {
+                fbalance_avant=0;
+                SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
+                etat = init;
+            }
+            break;
+
+    }
+
+
 }
\ No newline at end of file