carte esclave petit robot

Dependencies:   mbed Herkulex_Library_2019 actions_Gr ident_crac actions_Pr

Revision:
21:edf6d03fdb20
Parent:
20:42f8ec726ac8
Child:
22:9e92a2b941ba
--- a/main.cpp	Wed May 22 08:25:51 2019 +0000
+++ b/main.cpp	Wed May 22 11:19:27 2019 +0000
@@ -1,10 +1,5 @@
 #include "mbed.h"
-#include "ident_crac.h"
-#include "Capteur.h"
-#include "Actionneurs.h"
-#include "fonctions_herkulex.h"
 #include "main.h"
-#include "dt.h"
 
 #define SIZE_FIFO 50
 
@@ -16,45 +11,17 @@
 signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
 unsigned char EtatGameEnd=0;
 
-char fpresentoir_avant=0, fpresentoir_arriere=0;
-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,fbalance_arriere=0;
-char favant_relache=0,farriere_relache=0;
-char faccelerateur_avant=0,faccelerateur_arriere=0;;
+
+int rotation_couroies = 0;
+
+
+void canProcessRx(void);
 
 
 
-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);
-void automate_ventouse_presentoir_arriere (void);
-
-void automate_ventouse_goldenium_avant (void);
-void automate_ventouse_goldenium_arriere (void);
-
-void automate_ventouse_sol_avant (void);
-void automate_ventouse_sol_arriere(void);
-
-void automate_ventouse_sol_avant_relache(void);
-void automate_ventouse_sol_arriere_relache(void);
-
-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_accelerateur_avant(void);
-void automate_ventouse_accelerateur_arriere(void);
-
+#ifdef ROBOT_BIG
+#endif
 
 /*********************************************************************************************/
 /* FUNCTION NAME: canRx_ISR                                                                  */
@@ -71,85 +38,6 @@
     }
 
 }
-/*********************************************************************************************/
-/* FUNCTION NAME: SendAck                                                                    */
-/* DESCRIPTION  :                                                                            */
-/*********************************************************************************************/
-void SendAck(unsigned short from, unsigned short id) //FROM et ID sont inversés pour convenir à la lecture d'ACK de la carte principale
-{
-    CANMessage msgTx=CANMessage();
-    msgTx.id=from;                              //waitingAckFrom
-    msgTx.len=2;
-    msgTx.format=CANStandard;
-    msgTx.type=CANData;
-    // from sur 2 octets
-    msgTx.data[0]=(unsigned char)id;            //waitingAckID
-    msgTx.data[1]=(unsigned char)(id>>8);
-    can.write(msgTx);
-}
-/*********************************************************************************************/
-/* FUNCTION NAME: SendRawId                                                                  */
-/* DESCRIPTION  :                                                                            */
-/*********************************************************************************************/
-void SendRawId (unsigned short id)
-{
-    CANMessage msgTx=CANMessage();
-    msgTx.id=id;
-    msgTx.len=0;
-    can.write(msgTx); //carte esclave f446re
-    wait_us(200);
-}
-
-/*********************************************************************************************/
-/* FUNCTION NAME: GoStraight                                                                 */
-/* DESCRIPTION  : Transmission CAN correspondant à une ligne droite, avec ou sans recalage   */
-/*  recalage : 0 => pas de recalage                                                          */
-/*             1 => recalage en X                                                            */
-/*             2 => Recalage en Y                                                            */
-/*  newValue : Uniquement en cas de recalage, indique la nouvelle valeur de l'odo            */
-/*  isEnchainement : Indique si il faut executer l'instruction en enchainement               */
-/*                   0 => non                                                                */
-/*                   1 => oui                                                                */
-/*                   2 => dernière instruction de l'enchainement                             */
-/*********************************************************************************************/
-void GoStraight (signed short distance,unsigned char recalage, unsigned short newValue, unsigned char isEnchainement)
-{
-    CANMessage msgTx=CANMessage();
-    msgTx.id=ASSERVISSEMENT_RECALAGE;
-    msgTx.len=6;
-    msgTx.format=CANStandard;
-    msgTx.type=CANData;
-    // x sur 2 octets
-    msgTx.data[0]=(unsigned char)distance;
-    msgTx.data[1]=(unsigned char)(distance>>8);
-    //Recalage sur 1 octet
-    msgTx.data[2]=recalage;
-    //Valeur du recalage sur 2 octets
-    msgTx.data[3]=(unsigned char)newValue;
-    msgTx.data[4]=(unsigned char)(newValue>>8);
-    //Enchainement sur 1 octet
-    msgTx.data[5]=isEnchainement;
-
-    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()
 {
@@ -157,11 +45,28 @@
     servo_interrupt_en(); //permettre les interuptions
 
     wait(1);//attente servo boot
+    
+    
+#ifdef ROBOT_SMALL
     gabarit_petit_robot();
+#endif
+
+
+    
+#ifdef ROBOT_BIG
+    clear(doigt,2);
+    setTorque(doigt, TORQUE_ON,2);
+    positionControl(doigt,384,100,BLED_ON,2);
+    wait(1.0);
+#endif
+
+
     while(1) {
         canProcessRx();
         f_mesure();//dt35
 
+
+#ifdef ROBOT_SMALL
         automate_ventouse_presentoir_avant();
         automate_ventouse_presentoir_arriere();
 
@@ -182,6 +87,13 @@
 
         automate_ventouse_accelerateur_avant();
         automate_ventouse_accelerateur_arriere();
+#endif
+
+#ifdef ROBOT_BIG
+        fifo_couleur();
+        ascenseur();
+
+#endif
 
         if(EtatGameEnd==1) {
             while(1);
@@ -361,7 +273,7 @@
 
 #endif
 #ifdef ROBOT_BIG
-            case COUROIES:
+            case ASCENSEUR:
                 rotation_couroies = msgRxBuffer[FIFO_lecture].data[0];
                 break;
 
@@ -378,463 +290,6 @@
 
 }
 
-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();
-            can.write(CANMessage(HACHEUR_GET_PRESENTOIR_AV));
-            SendRawId(HACHEUR_STATUT_VENTOUSES);
-            etat = attente_ack_ventouse;
-            break;
-
-        case attente_ack_ventouse:
-            if((status_pompe&MASK_PRESENTOIR_AV)==MASK_PRESENTOIR_AV) {
-                fpresentoir_avant=0;
-                SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
-                etat = init;
-            }
-            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();
-            can.write(CANMessage(HACHEUR_GET_PRESENTOIR_AR));
-            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;
-
-    }
-}
-void automate_ventouse_goldenium_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(fgoldenium_avant)
-                etat=envoi_instruction;
-            break;
-
-        case envoi_instruction://envoi instruction
-            SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
-            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:
-            if((status_pompe&MASK_GOLDENIUM_AV)== MASK_GOLDENIUM_AV) {
-                fgoldenium_avant=0;
-                SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
-                etat = init;
-            }
-            break;
-
-    }
-}
-
-void automate_ventouse_goldenium_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(fgoldenium_arriere)
-                etat=envoi_instruction;
-            break;
-
-        case envoi_instruction://envoi instruction
-            SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
-            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:
-            if((status_pompe&MASK_GOLDENIUM_AR)== MASK_GOLDENIUM_AR) {
-                fgoldenium_arriere=0;
-                SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
-                etat = init;
-            }
-            break;
-
-    }
-
-
-}
-
-void automate_ventouse_sol_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(fsol_avant)
-                etat=envoi_instruction;
-            break;
-
-        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;
-
-        case attente_ack_ventouse:
-            if((status_pompe&MASK_SOL_AV)== MASK_SOL_AV) {
-                sol_avant_remonter();
-                fsol_avant=0;
-                SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
-                etat = init;
-            }
-            break;
-
-    }
-
-
-}
-void automate_ventouse_sol_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(fsol_arriere)
-                etat=envoi_instruction;
-            break;
-
-        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;
-
-        case attente_ack_ventouse:
-            if((status_pompe&MASK_SOL_AR)== MASK_SOL_AR) {
-                sol_arriere_remonter();
-                fsol_arriere=0;
-                SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
-                etat = init;
-            }
-            break;
-
-    }
 
 
-}
-void automate_ventouse_balance_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(fbalance_avant)
-                etat=envoi_instruction;
-            break;
-
-        case envoi_instruction://envoi instruction
-            SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
-            presentoir_avant();
-            verification();
-            balance_avant_placement();
-            Rotate(200);
-            wait(0.5);
-            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));
-            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);
-            presentoir_arriere();
-            verification();
-            balance_arriere_placement();
-            Rotate(200);
-            wait(0.5);
-            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));
-            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_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);
-            SendRawId(HACHEUR_STATUT_VENTOUSES);
-            etat = attente_ack_ventouse;
-            break;
-
-        case attente_ack_ventouse:
-            SendRawId(HACHEUR_RELEASE_AV);
-            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;
-    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);
-            SendRawId(HACHEUR_STATUT_VENTOUSES);
-            etat = attente_ack_ventouse;
-            break;
-
-        case attente_ack_ventouse:
-            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)
-{
-    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();
-            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));
-            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();
-            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));
-            if((status_pompe&MASK_SOL_AR)== 0) {
-                sol_arriere_remonter();
-                SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
-                fsol_arriere_relache=0;
-                etat = init;
-            }
-            break;
-
-    }
-
-
-}
-void automate_ventouse_accelerateur_avant (void)
-{
-    typedef enum {init,envoi_instruction,attente_ack_ventouse_droite_gauche,attente_ack_ventouse_centre} type_etat;
-    static type_etat etat = init;
-
-    switch(etat) {
-        case init: //attente d'initialisation
-            if(faccelerateur_avant)
-                etat=envoi_instruction;
-            break;
-
-        case envoi_instruction://envoi instruction
-            SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
-            accelerateur_avant();
-            GoStraight(200,1,35,0);//35epaisseur accelerateur
-            SendRawId(HACHEUR_STATUT_VENTOUSES);
-            etat = attente_ack_ventouse_droite_gauche;
-            break;
-
-        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));
-            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();
-                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_accelerateur_arriere (void)
-{
-
-}
-
-