Diff: main.cpp

Revision:

16:2a66cfe37262

Parent:

15:119fc452dcaf

Child:

17:b8ffd3593d0e

Child:

18:0ea0d887966f

--- a/main.cpp	Tue May 21 15:07:08 2019 +0000
+++ b/main.cpp	Tue May 21 15:27:07 2019 +0000
@@ -438,11 +438,11 @@
             goldenium_avant();
             char pompe=AV_CENTRE;
             can.write(CANMessage(HACHEUR_GET_ATOM, &pompe,1));
+             SendRawId(HACHEUR_STATUT_VENTOUSES);
             etat = attente_ack_ventouse;
             break;
 
-        case attente_ack_ventouse:
-            SendRawId(HACHEUR_STATUT_VENTOUSES);
+        case attente_ack_ventouse:         
             if((status_pompe&MASK_GOLDENIUM_AV)== MASK_GOLDENIUM_AV) {
                 fgoldenium_avant=0;
                 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
@@ -470,11 +470,11 @@
             goldenium_arriere();
             char pompe=AR_CENTRE;
             can.write(CANMessage(HACHEUR_GET_ATOM, &pompe,1));
+             SendRawId(HACHEUR_STATUT_VENTOUSES);
             etat = attente_ack_ventouse;
             break;
 
-        case attente_ack_ventouse:
-            SendRawId(HACHEUR_STATUT_VENTOUSES);
+        case attente_ack_ventouse:      
             if((status_pompe&MASK_GOLDENIUM_AR)== MASK_GOLDENIUM_AR) {
                 fgoldenium_arriere=0;
                 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
@@ -575,13 +575,13 @@
             balance_avant();
             Rotate(-200);
             wait(0.5);
+             SendRawId(HACHEUR_STATUT_VENTOUSES);
             etat = attente_ack_ventouse;
             break;
 
         case attente_ack_ventouse:
             char pompe=AV_DROIT;
             can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
-            SendRawId(HACHEUR_STATUT_VENTOUSES);
             if((status_pompe&MASK_AV_DROIT)== 0) {
                 fbalance_avant=0;
                 presentoir_arriere();
@@ -616,13 +616,13 @@
             balance_arriere();
             Rotate(-200);
             wait(0.5);
+            SendRawId(HACHEUR_STATUT_VENTOUSES);
             etat = attente_ack_ventouse;
             break;
 
         case attente_ack_ventouse:
             char pompe=AR_DROIT;
             can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
-            SendRawId(HACHEUR_STATUT_VENTOUSES);
             if((status_pompe&MASK_AR_DROIT)== 0) {
                 fbalance_arriere=0;
                 presentoir_arriere();
@@ -650,12 +650,12 @@
 
         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);
-            SendRawId(HACHEUR_STATUT_VENTOUSES);
             if((status_pompe&MASK_PRESENTOIR_AV)== 0) {
                 favant_relache=0;
                 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
@@ -680,22 +680,20 @@
 
         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_AR);
-            SendRawId(HACHEUR_STATUT_VENTOUSES);
+            SendRawId(HACHEUR_RELEASE_AR);  
             if((status_pompe&MASK_PRESENTOIR_AR)== 0) {
                 farriere_relache=0;
                 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
                 etat = init;
             }
             break;
-
     }
 
-
 }
 void automate_ventouse_sol_avant_relache (void)
 {
@@ -712,13 +710,13 @@
             SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
             positionControl(AV_sol,520,1,BLED_ON,4);//baisser
             verification();
+            SendRawId(HACHEUR_STATUT_VENTOUSES);
             etat = attente_ack_ventouse;
             break;
 
         case attente_ack_ventouse:
             char pompe=AV_BAS;
             can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
-            SendRawId(HACHEUR_STATUT_VENTOUSES);
             if((status_pompe&MASK_SOL_AV)== 0) {
                 sol_avant_remonter();
                 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
@@ -747,13 +745,13 @@
             SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
             positionControl(AR_sol,480,1,BLED_ON,4);//baisser
             verification();
+             SendRawId(HACHEUR_STATUT_VENTOUSES);
             etat = attente_ack_ventouse;
             break;
 
         case attente_ack_ventouse:
             char pompe=AR_BAS;
             can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
-            SendRawId(HACHEUR_STATUT_VENTOUSES);
             if((status_pompe&MASK_SOL_AR)== 0) {
                 sol_arriere_remonter();
                 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
@@ -768,7 +766,7 @@
 }
 void automate_ventouse_accelerateur_avant (void)
 {
-    typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
+    typedef enum {init,envoi_instruction,attente_ack_ventouse_droite_gauche,attente_ack_ventouse_centre} type_etat;
     static type_etat etat = init;
 
     switch(etat) {
@@ -781,15 +779,24 @@
             SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
             accelerateur_avant();
             GoStraight(200,1,35,0);//35epaisseur accelerateur
-            etat = attente_ack_ventouse;
+            SendRawId(HACHEUR_STATUT_VENTOUSES);
+            etat = attente_ack_ventouse_droite_gauche;
             break;
 
-        case attente_ack_ventouse:
+        case attente_ack_ventouse_droite_gauche:
             char pompe=AV_DROIT;
             can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
             pompe=AV_GAUCHE;
             can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
-            SendRawId(HACHEUR_STATUT_VENTOUSES);
+            if((status_pompe&MASK_AV_DROIT_GAUCHE)== 0) {
+                GoStraight(-20,0,0,0);
+                wait(1);
+                presentoir_avant();
+                etat = attente_ack_ventouse_centre;
+            }
+        case attente_ack_ventouse_centre:
+             pompe=AV_CENTRE;
+             can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));
             if((status_pompe&MASK_AV_DROIT_GAUCHE)== 0) {
                 faccelerateur_avant=0;
                 presentoir_avant();
@@ -797,6 +804,7 @@
                 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
                 etat = init;
             }
+        
             break;
 
     }