Diff: main.cpp

Revision:

12:191486ba5118

Parent:

11:6c11c081c855

Child:

13:9e19048eb065

diff -r 6c11c081c855 -r 191486ba5118 main.cpp
--- a/main.cpp	Mon May 20 11:38:43 2019 +0000
+++ b/main.cpp	Mon May 20 17:34:39 2019 +0000
@@ -20,7 +20,10 @@
 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 fbalance_avant=0,fbalance_arriere=0;
+char favant_relache=0,farriere_relache=0;
+
+
 char status_pompe=0;
 
 void canRx_ISR (void);
@@ -42,6 +45,10 @@
 void automate_ventouse_sol_arriere_relache(void);
 
 void automate_ventouse_balance_avant (void);
+void automate_ventouse_balance_arriere(void);
+
+
+void automate_ventouse_relache_arriere (void);
 /*********************************************************************************************/
 /* FUNCTION NAME: canRx_ISR                                                                  */
 /* DESCRIPTION  : lit les messages sur le can et les stocke dans la FIFO                     */
@@ -119,6 +126,23 @@
     can.write(msgTx);
     //wait_ms(500);
 }
+/****************************************************************************************/
+/* FUNCTION NAME: Rotate                                                                */
+/* DESCRIPTION  : Transmission CAN correspondant à une rotation                         */
+/****************************************************************************************/
+void Rotate (signed short angle)
+{
+    CANMessage msgTx=CANMessage();
+    msgTx.id=ASSERVISSEMENT_ROTATION;  // Tx rotation autour du centre du robot
+    msgTx.len=2;
+    msgTx.format=CANStandard;
+    msgTx.type=CANData;
+    //  Angle signé sur 2 octets
+    msgTx.data[0]=(unsigned char)angle;
+    msgTx.data[1]=(unsigned char)(angle>>8);
+
+    can.write(msgTx);
+}
 
 int main()
 {
@@ -141,6 +165,10 @@
         automate_ventouse_sol_arriere();
 
         automate_ventouse_balance_avant();
+        automate_ventouse_balance_arriere();
+
+
+        automate_ventouse_relache_arriere();
         
         if(EtatGameEnd==1) {
             while(1);
@@ -232,9 +260,7 @@
                 break;
 
             case BALANCE_ARRIERE:
-                SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
-                balance_arriere();
-                SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
+                fbalance_arriere=1;
                 break;
 
             case ACCELERATEUR_AVANT:
@@ -272,6 +298,14 @@
             case SOL_ARRIERE_RELACHE:
                 fsol_arriere_relache=1;
                 break;
+            case AVANT_RELACHE:
+                favant_relache=1;
+                break;
+
+            case ARRIERE_RELACHE:
+                farriere_relache=1;
+                break;
+                
 //--------------------------------------------------------------------------ACK carte pompe----------------------------------------------
 
 
@@ -507,14 +541,94 @@
 
         case envoi_instruction://envoi instruction
             SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
+            balance_avant_placement();
+            Rotate(200);
+            wait(0.5);
             balance_avant();
-            SendRawId(HACHEUR_STATUT_VENTOUSES);
+            Rotate(-200);
+            wait(0.5);
             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();
+                presentoir_avant();
+                can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));//on arrete la pompe car elle est réactivé par presentoir_avant();
+                SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
+                etat = init;
+            }
+            break;
+
+    }
+
+
+}
+void automate_ventouse_balance_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(fbalance_arriere)
+                etat=envoi_instruction;
+            break;
+
+        case envoi_instruction://envoi instruction
+            SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
+            balance_arriere_placement();
+            Rotate(200);
+            wait(0.5);
+            balance_arriere();
+            Rotate(-200);
+            wait(0.5);
+            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();
+                presentoir_avant();
+                can.write(CANMessage(HACHEUR_RELEASE_ATOM, &pompe,1));//on arrete la pompe car elle est réactivé par presentoir_avant();
+                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;
+    static type_etat etat = init;
+
+    switch(etat) {
+        case init: //attente d'initialisation
+            if(farriere_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_AR);
+            SendRawId(HACHEUR_STATUT_VENTOUSES);
+            if((status_pompe&MASK_PRESENTOIR_AR)== 0) {
+                farriere_relache=0;
                 SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
                 etat = init;
             }