File content as of revision 12:191486ba5118:

#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

CAN can(PB_8,PB_9,1000000); // Rx&Tx pour le CAN


CANMessage msgRxBuffer[SIZE_FIFO];
unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
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 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_arriere (void);
/*********************************************************************************************/
/* FUNCTION NAME: canRx_ISR                                                                  */
/* DESCRIPTION  : lit les messages sur le can et les stocke dans la FIFO                     */
/*********************************************************************************************/
void canRx_ISR (void)
{
    if (can.read(msgRxBuffer[FIFO_ecriture])) {
        if (msgRxBuffer[FIFO_ecriture].id==GLOBAL_GAME_END) {

        }
        FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;

    }

}
/*********************************************************************************************/
/* 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()
{
    can.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN
    servo_interrupt_en(); //permettre les interuptions

    wait(1);//attente servo boot
    gabarit_petit_robot();
    while(1) {
        canProcessRx();
        f_mesure();//dt35

        automate_ventouse_presentoir_avant();
        automate_ventouse_presentoir_arriere();

        automate_ventouse_goldenium_avant();
        automate_ventouse_goldenium_arriere();

        automate_ventouse_sol_avant();
        automate_ventouse_sol_arriere();

        automate_ventouse_balance_avant();
        automate_ventouse_balance_arriere();


        automate_ventouse_relache_arriere();
        
        if(EtatGameEnd==1) {
            while(1);
        }

    }
}

/****************************************************************************************/
/* FUNCTION NAME: canProcessRx                                                          */
/* DESCRIPTION  : Fonction de traitement des messages CAN                               */
/****************************************************************************************/
void canProcessRx(void)
{
    static signed char FIFO_occupation=0,FIFO_max_occupation=0;
    CANMessage msgTx=CANMessage();
    FIFO_occupation=FIFO_ecriture-FIFO_lecture;
    if(FIFO_occupation<0)
        FIFO_occupation=FIFO_occupation+SIZE_FIFO;
    if(FIFO_max_occupation<FIFO_occupation)
        FIFO_max_occupation=FIFO_occupation;
    if(FIFO_occupation!=0) {
        int identifiant=msgRxBuffer[FIFO_lecture].id;

        switch(identifiant) {

            case GLOBAL_GAME_END:
                EtatGameEnd = 1;
                break;

            case DATA_TELEMETRE: //Lit le telemetre N°X suivant la data dans le CAN
                char numero_telemetre=msgRxBuffer[FIFO_lecture].data[0];
                short distance=lecture_telemetre(numero_telemetre);


                msgTx.id=RECEPTION_DATA; // tx Valeur Telemetre1
                msgTx.len=2;
                msgTx.format=CANStandard;
                msgTx.type=CANData;
                // Rayon sur 2 octets
                msgTx.data[0]=(unsigned char)distance;
                msgTx.data[1]=(unsigned char)(distance>>8);

                can.write(msgTx);
                SendAck(ACKNOWLEDGE_TELEMETRE,RECEPTION_DATA);
                break;

            case DATA_RECALAGE:
                short distance1=lecture_telemetre(1);
                short distance2=lecture_telemetre(2);
                short distance3=lecture_telemetre(3);
                short distance4=lecture_telemetre(4);

                msgTx.id=RECEPTION_RECALAGE; // tx Valeur Telemetre1
                msgTx.len=8;
                msgTx.format=CANStandard;
                msgTx.type=CANData;
                // Rayon sur 2 octets
                msgTx.data[0]=(unsigned char)distance1;
                msgTx.data[1]=(unsigned char)(distance1>>8);
                msgTx.data[2]=(unsigned char)distance2;
                msgTx.data[3]=(unsigned char)(distance2>>8);
                msgTx.data[4]=(unsigned char)distance3;
                msgTx.data[5]=(unsigned char)(distance3>>8);
                msgTx.data[6]=(unsigned char)distance4;
                msgTx.data[7]=(unsigned char)(distance4>>8);
                can.write(msgTx);
                SendAck(ACKNOWLEDGE_TELEMETRE,RECEPTION_RECALAGE);
                break;

#ifdef ROBOT_SMALL
//-------------------------------------------------------------------------Actions petit robot----------------------------------------------
            case GABARIT_PETIT_ROBOT:
                SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
                gabarit_petit_robot();
                SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
                break;

            case PRESENTOIR_AVANT:
                fpresentoir_avant=1;
                break;

            case PRESENTOIR_ARRIERE:
                fpresentoir_arriere=1;
                break;

            case BALANCE_AVANT:
                fbalance_avant=1;
                break;

            case BALANCE_ARRIERE:
                fbalance_arriere=1;
                break;

            case ACCELERATEUR_AVANT:
                SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
                accelerateur_avant();
                SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
                break;

            case ACCELERATEUR_ARRIERE:
                SendAck(ACKNOWLEDGE_HERKULEX, ACK_ACTION);
                accelerateur_arriere();
                SendAck(ACKNOWLEDGE_HERKULEX, ACK_FIN_ACTION);
                break;

            case GOLDENIUM_AVANT:
                fgoldenium_avant=1;
                break;

            case GOLDENIUM_ARRIERE:
                fgoldenium_arriere=1;
                break;

            case SOL_AVANT:
                fsol_avant=1;
                break;

            case SOL_ARRIERE:
                fsol_arriere=1;
                break;

            case SOL_AVANT_RELACHE:
                fsol_avant_relache=1;
                break;

            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----------------------------------------------


            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 :
                status_pompe &= ~(0x01 << msgRxBuffer[FIFO_lecture].data[0]);
                break;
//-------------------------------------------------------------------------------------------------------------------------------------------


#endif
#ifdef ROBOT_BIG
            case 3:

                break;

            case 5:

                break;

#endif
        }
        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();
            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();
            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();
            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();
            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();
            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();
            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);
            balance_avant_placement();
            Rotate(200);
            wait(0.5);
            balance_avant();
            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;
            }
            break;

    }


}