Diff: main.cpp

Revision:

4:4a79942713fa

Parent:

2:9e63099cca99

Child:

5:6e198cdd99ad

--- a/main.cpp	Fri May 10 10:56:49 2019 +0000
+++ b/main.cpp	Fri May 10 19:16:33 2019 +0000
@@ -15,11 +15,16 @@
 signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
 unsigned char EtatGameEnd=0;
 
+char fpresentoir_avant=0;
+
+char status_pompe=0;
+
 void canRx_ISR (void);
 void SendAck(unsigned short id, unsigned short from);
 void SendRawId (unsigned short id);
 void GoStraight (signed short distance,unsigned char recalage, unsigned short newValue, unsigned char isEnchainement);
 void canProcessRx(void);
+void automate_ventouse_presentoir_avant(void);
 
 /*********************************************************************************************/
 /* FUNCTION NAME: canRx_ISR                                                                  */
@@ -108,9 +113,7 @@
 
     while(1) {
         canProcessRx();
-
-
-
+        automate_ventouse_presentoir_avant();
         if(EtatGameEnd==1) {
             while(1);
         }
@@ -181,6 +184,7 @@
                 break;
 
 #ifdef ROBOT_SMALL
+//-------------------------------------------------------------------------Actions petit robot----------------------------------------------
             case GABARIT_PETIT_ROBOT:
                 SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
                 gabarit_petit_robot();
@@ -188,9 +192,7 @@
                 break;
 
             case PRESENTOIR_AVANT:
-                SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
-                presentoir_avant();
-                SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
+                fpresentoir_avant=1;
                 break;
 
             case PRESENTOIR_ARRIERE:
@@ -256,6 +258,23 @@
                 sol_arriere_relache();
                 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
                 break;
+//--------------------------------------------------------------------------ACK carte pompe----------------------------------------------
+
+
+            case HACHEUR_STATUT_VENTOUSES:
+                status_pompe = msgRxBuffer[FIFO_lecture].data[1];
+
+                break;
+
+            case HACHEUR_GET_ATOM_ACK:
+                status_pompe |= (0x01 << msgRxBuffer[FIFO_lecture].data[0]);
+                break;
+
+            case HACHEUR_RELEASE_ATOM_ACK :
+                status_pompe &= ~(0x01 << msgRxBuffer[FIFO_lecture].data[0]);
+                break;
+//-------------------------------------------------------------------------------------------------------------------------------------------
+
 
 #endif
 #ifdef ROBOT_BIG
@@ -272,4 +291,41 @@
         FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
 
     }
+
+
+}
+
+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();
+            SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
+
+            etat = attente_ack_ventouse;
+            break;
+
+        case attente_ack_ventouse:
+            if(status_pompe&MASK_PRESENTOIR_AV==MASK_PRESENTOIR_AV) {
+                fpresentoir_avant=0;
+                etat = init;
+            } else {
+                SendRawId(HACHEUR_STATUT_VENTOUSES);
+            }
+            break;
+
+    }
+
+
+
 }
\ No newline at end of file