carte esclave petit robot

Dependencies:   mbed Herkulex_Library_2019 actions_Gr ident_crac actions_Pr

Revision:
13:9e19048eb065
Parent:
12:191486ba5118
Child:
14:b1abd2f6df75
--- a/main.cpp	Mon May 20 17:34:39 2019 +0000
+++ b/main.cpp	Tue May 21 08:56:49 2019 +0000
@@ -24,6 +24,7 @@
 char favant_relache=0,farriere_relache=0;
 
 
+
 char status_pompe=0;
 
 void canRx_ISR (void);
@@ -47,8 +48,11 @@
 void automate_ventouse_balance_avant (void);
 void automate_ventouse_balance_arriere(void);
 
+void automate_ventouse_relache_avant(void);
+void automate_ventouse_relache_arriere (void);
 
-void automate_ventouse_relache_arriere (void);
+void automate_ventouse_sol_avant_relache (void);
+void automate_ventouse_sol_arriere_relache (void);
 /*********************************************************************************************/
 /* FUNCTION NAME: canRx_ISR                                                                  */
 /* DESCRIPTION  : lit les messages sur le can et les stocke dans la FIFO                     */
@@ -167,9 +171,12 @@
         automate_ventouse_balance_avant();
         automate_ventouse_balance_arriere();
 
-
+        automate_ventouse_relache_avant();
         automate_ventouse_relache_arriere();
         
+        automate_ventouse_sol_avant_relache();
+        automate_ventouse_sol_arriere_relache(); 
+
         if(EtatGameEnd==1) {
             while(1);
         }
@@ -305,7 +312,8 @@
             case ARRIERE_RELACHE:
                 farriere_relache=1;
                 break;
-                
+
+
 //--------------------------------------------------------------------------ACK carte pompe----------------------------------------------
 
 
@@ -322,6 +330,8 @@
             case HACHEUR_RELEASE_ATOM_ACK :
                 status_pompe &= ~(0x01 << msgRxBuffer[FIFO_lecture].data[0]);
                 break;
+
+
 //-------------------------------------------------------------------------------------------------------------------------------------------
 
 
@@ -479,6 +489,8 @@
         case envoi_instruction://envoi instruction
             SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
             sol_avant_baisser();
+            char pompe=AV_BAS;
+            can.write(CANMessage(HACHEUR_GET_ATOM, &pompe,1));
             SendRawId(HACHEUR_STATUT_VENTOUSES);
             etat = attente_ack_ventouse;
             break;
@@ -511,6 +523,8 @@
         case envoi_instruction://envoi instruction
             SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
             sol_arriere_baisser();
+            char pompe=AR_BAS;
+            can.write(CANMessage(HACHEUR_GET_ATOM, &pompe,1));
             SendRawId(HACHEUR_STATUT_VENTOUSES);
             etat = attente_ack_ventouse;
             break;
@@ -608,6 +622,36 @@
 
 
 }
+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);
+            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);
+                etat = init;
+            }
+            break;
+
+    }
+
+
+}
 void automate_ventouse_relache_arriere (void)
 {
     typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
@@ -637,4 +681,73 @@
     }
 
 
+}
+void automate_ventouse_sol_avant_relache (void)
+{
+    typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
+    static type_etat etat = init;
+
+    switch(etat) {
+        case init: //attente d'initialisation
+            if(fsol_avant_relache)
+                etat=envoi_instruction;
+            break;
+
+        case envoi_instruction://envoi instruction
+            SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
+            positionControl(AV_sol,520,1,BLED_ON,4);//baisser
+            verification();
+            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);
+                fsol_avant_relache=0;
+                etat = init;
+            }
+            break;
+
+    }
+
+
+}
+
+void automate_ventouse_sol_arriere_relache (void)
+{
+    typedef enum {init,envoi_instruction,attente_ack_ventouse} type_etat;
+    static type_etat etat = init;
+
+    switch(etat) {
+        case init: //attente d'initialisation
+            if(fsol_arriere_relache)
+                etat=envoi_instruction;
+            break;
+
+        case envoi_instruction://envoi instruction
+            SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
+            positionControl(AR_sol,480,1,BLED_ON,4);//baisser
+            verification();
+            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);
+                fsol_arriere_relache=0;
+                etat = init;
+            }
+            break;
+
+    }
+
+
 }
\ No newline at end of file