carte esclave petit robot

Dependencies:   mbed Herkulex_Library_2019 actions_Gr ident_crac actions_Pr

Revision:
5:6e198cdd99ad
Parent:
4:4a79942713fa
Child:
6:45f9cf44718a
--- a/main.cpp	Fri May 10 19:16:33 2019 +0000
+++ b/main.cpp	Tue May 14 15:09:14 2019 +0000
@@ -16,6 +16,7 @@
 unsigned char EtatGameEnd=0;
 
 char fpresentoir_avant=0;
+char fpresentoir_arriere=0;
 
 char status_pompe=0;
 
@@ -25,6 +26,7 @@
 void GoStraight (signed short distance,unsigned char recalage, unsigned short newValue, unsigned char isEnchainement);
 void canProcessRx(void);
 void automate_ventouse_presentoir_avant(void);
+void automate_ventouse_presentoir_arriere (void);
 
 /*********************************************************************************************/
 /* FUNCTION NAME: canRx_ISR                                                                  */
@@ -114,6 +116,7 @@
     while(1) {
         canProcessRx();
         automate_ventouse_presentoir_avant();
+        automate_ventouse_presentoir_arriere();
         if(EtatGameEnd==1) {
             while(1);
         }
@@ -196,9 +199,7 @@
                 break;
 
             case PRESENTOIR_ARRIERE:
-                SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
-                presentoir_arriere();
-                SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
+                fpresentoir_arriere=1;
                 break;
 
             case BALANCE_AVANT:
@@ -263,11 +264,12 @@
 
             case HACHEUR_STATUT_VENTOUSES:
                 status_pompe = msgRxBuffer[FIFO_lecture].data[1];
-
+                //can.write(CANMessage(0x529, &status_pompe,1));
                 break;
 
             case HACHEUR_GET_ATOM_ACK:
                 status_pompe |= (0x01 << msgRxBuffer[FIFO_lecture].data[0]);
+                //can.write(CANMessage(0x529, &status_pompe,1));
                 break;
 
             case HACHEUR_RELEASE_ATOM_ACK :
@@ -310,17 +312,44 @@
         case envoi_instruction://envoi instruction
             SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
             presentoir_avant();
-            SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
-
+            SendRawId(HACHEUR_STATUT_VENTOUSES);
             etat = attente_ack_ventouse;
             break;
 
         case attente_ack_ventouse:
-            if(status_pompe&MASK_PRESENTOIR_AV==MASK_PRESENTOIR_AV) {
+            if((status_pompe&MASK_PRESENTOIR_AV)==MASK_PRESENTOIR_AV) {
                 fpresentoir_avant=0;
+                SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
                 etat = init;
-            } else {
-                SendRawId(HACHEUR_STATUT_VENTOUSES);
+            }
+            break;
+
+    }
+}
+void automate_ventouse_presentoir_arriere (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_arriere)
+                etat=envoi_instruction;
+            break;
+
+        case envoi_instruction://envoi instruction
+            SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
+            presentoir_arriere();
+            SendRawId(HACHEUR_STATUT_VENTOUSES);
+            etat = attente_ack_ventouse;
+            break;
+
+        case attente_ack_ventouse:
+            if((status_pompe&MASK_PRESENTOIR_AR)==MASK_PRESENTOIR_AR) {
+                fpresentoir_arriere=0;
+                SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
+                etat = init;
             }
             break;