carte esclave petit robot

Dependencies:   mbed Herkulex_Library_2019 actions_Gr ident_crac actions_Pr

main.cpp

Committer:
Artiom
Date:
2019-05-06
Revision:
0:bc74da1c502f
Child:
1:568955af8c2b

File content as of revision 0:bc74da1c502f:

#include "mbed.h"
#include "ident_crac.h"
#include "Capteur.h"
#include "Actionneurs.h"
#include "fonctions_herkulex.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;

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

/*********************************************************************************************/
/* 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);
}

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
    init_petit_robot();

    while(1) {
        canProcessRx();

        init_petit_robot();

        presentoir_avant();
        presentoir_arriere();

        balance_avant();
        balance_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
            case INIT_PETIT_ROBOT:
                init_petit_robot();
                break;

            case PRESENTOIR_AVANT:
                presentoir_avant();
                break;

            case PRESENTOIR_ARRIERE:
                presentoir_arriere();
                break;

            case BALANCE_AVANT:
                balance_avant();
                break;

            case BALANCE_ARRIERE:
                balance_arriere();
                break;

#endif
#ifdef ROBOT_BIG
            case 3:

                break;

            case 5:

                break;

#endif
        }
        FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;

    }
}