File content as of revision 29:41e02746041d:

#include "global.h"

//#include "StrategieManager.h"



#define M_PI 3.14159265358979323846
#define BLEU 0xFF1467E5
#define ROUGE 0xFFFF0000
#define VERT 0xFF00FF00
#define JAUNE 0xFFFEFE00
#define BLANC 0xFF000000
#define DIY_GREY 0xFFDFDFDF 

char tableau_aff[10][50];
char tableau_etat[22][50]=
{
    "Check_carte_screen",
    "Check_carte_screen_wait_ack",
    "Check_cartes",
    "Check_cartes_wait_ack",
    "Wait_force",
    "Config",
    "Game_init",
    "Game_wait_for_jack",
    "Game_start",
    "Game_next_instruction",
    "Game_instruction",
    "Game_wait_ack",
    "Game_jump_time",
    "Game_jump_config",
    "Game_jump_position",
    "Game_wait_end_instruction",
    "Warning_timeout",
    "Waring_end_balise_wait",
    "Warning_end_last_instruction",
    "Warning_switch_strategie",
    "End",
    "End_loop",
};



Ticker ticker;
TS_DISCO_F469NI ts;
LCD_DISCO_F469NI lcd;

TS_StateTypeDef TS_State;

Ticker chrono;
Timeout AffTime;
Timer timer;
Timer cartesCheker;//Le timer pour le timeout de la vérification des cartes
Timer gameTimer;
Timer debugetatTimer;
Timer timeoutWarning;
Timer timeoutWarningWaitEnd;
Timeout chronoEnd;//permet d'envoyer la trame CAN pour la fin 

unsigned char screenChecktry = 0;
unsigned char test[32] = {32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32};

char counter = 0;
char check;
char Jack = 1;

int flag = 0, flag_strat = 0, flag_timer;
char Ack_strat = 0;
signed char Strat = 0;
signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN

signed short x_robot,y_robot,theta_robot;//La position du robot
signed short target_x_robot, target_y_robot, target_theta_robot;
E_InstructionType actionPrecedente;
signed short start_move_x,start_move_y,start_move_theta;//La position du robot lors du début d'un mouvement, utilisé pour reprendre le mouvement apres stop balise
//unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
int flagSendCan=1;
unsigned char Cote = 0; //0 -> bleu | 1 -> jaune

unsigned char checkCurrent = 0;
unsigned char countAliveCard = 0; 

signed char Strategie = 0; //N° de la strategie (1-10)

unsigned char ModeDemo = 0; // Si à 1, indique que l'on est dans le mode demo

unsigned char countRobotNear = 0;//Le nombre de robot à proximité

unsigned char ingnorBaliseOnce = 0;

unsigned char ingnorInversionOnce = 0;//Pour ignorer l'inversion des instruction une fois

struct S_Instruction instruction;

float cptf; 
int cpt,cpt1;  

typedef enum {INIT, ATT, CHOIX, DEMO, DEMO_EN_COURS, SELECT_SIDE, TACTIQUE, DETAILS,LECTURE, LAUNCH, AFF_WAIT_JACK, WAIT_JACK, COMPTEUR, FIN} T_etat;
T_etat etat = INIT;
E_stratGameEtat     gameEtat  = ETAT_CHECK_CARTES;
E_stratGameEtat     lastEtat  = ETAT_CHECK_CARTES;


/////////////////DEFINITION DES BOUTONS////////////////////
    Button COTE_BLEU(0, 25, 400, 300, "Bleu");
    Button COTE_JAUNE(0, 350, 400, 300, "Jaune");
    Button RETOUR  (0, 680, 400, 110, "--Precedent--");
    Button LANCER  (0, 200, 400, 200, "--LANCER--");
    Button CHECK (0, 420, 400, 200, "Valider");
    Button MATCH (0, 50, 400, 320, "Match");
    Button DEMONSTRATION (0, 400, 400, 320, "Demo");
    Button TEST_MOTEUR(0, 25, 400, 200, "Moteur");
    Button TEST_BRAS(0, 250, 400, 200, "Bras");
    Button FORCE_LAUNCH(0, 50, 400, 320, "Force Launch");
    Button SUIVANT(0,380,200,100,"Suivant");
    ////////////////////////////////////////////////////////////

void affichage_debug(char *bug);
void SendRawId (unsigned short id);
void SelectionStrat (unsigned char numeroStrat);
void Setflag(void);
void can2Rx_ISR(void);
signed char Bouton_Strat (void);

void print_segment(int nombre, int decalage);
void affichage_compteur (int nombre);
void effacer_segment(long couleur);


#ifdef ROBOT_BIG


unsigned short id_check[NOMBRE_CARTES]= {CHECK_MOTEUR,CHECK_BALISE,CHECK_ACTIONNEURS_AVANT,CHECK_ACTIONNEURS_ARRIERE};
unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_MOTEUR,ALIVE_BALISE,ALIVE_ACTIONNEURS_AVANT,ALIVE_ACTIONNEURS_ARRIERE};

InterruptIn jack(PA_6); //  entrée analogique en interruption pour le jack
#else


unsigned short id_check[NOMBRE_CARTES]= {CHECK_MOTEUR};
unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_MOTEUR};


#endif


    
    


/****************************************************************************************/
/* FUNCTION NAME: chronometre_ISR                                                       */
/* DESCRIPTION  : Interruption à la fin des 90s du match                                */
/****************************************************************************************/
void chronometre_ISR (void)
{
    SendRawId(ASSERVISSEMENT_STOP);//On stope les moteurs
    SendRawId(GLOBAL_GAME_END);//Indication fin de match
    etat=FIN;
    gameTimer.stop();//Arret du timer

#ifdef ROBOT_BIG
    wait_ms(2000);
    doFunnyAction();
#endif
    
    while(1);//On bloque la programme dans l'interruption
}



/****************************************************************************************/
/* FUNCTION NAME: jack_ISR                                                              */
/* DESCRIPTION  : Interruption en changement d'état sur le Jack                         */
/****************************************************************************************/
void jack_ISR (void)
{
    if(gameEtat == ETAT_GAME_WAIT_FOR_JACK) {
        gameEtat = ETAT_GAME_START;//On débute le match
        etat=COMPTEUR;
    }
}

/****************************************************************************************/
/* FUNCTION NAME: SelectionStrat                                                        */
/* DESCRIPTION  : Affiche la Stratégie sélectionnée sur l'ihm                           */
/****************************************************************************************/


void SelectionStrat (unsigned char Strategie)
{
    lcd.SetBackColor(LCD_COLOR_WHITE);
    lcd.SetTextColor(LCD_COLOR_BLACK);
     
    switch (Strategie+1)
    {
        case 0x1 :
            //description de Strategie n°1
            lcd.DisplayStringAt(150, 0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
            break;
            
        case 0x2 :
            //description de Strategie n°2
            lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
            break;
            
        case 0x3 :
            //description de Strategie n°3
            lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
            break;
            
        case 0x4 :
            //description de Strategie n°4
            lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
            break;
            
        case 0x5 :
            //description de Strategie n°5
            lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
            break;
            
        case 0x6 :
            //description de Strategie n°5
            lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
            break;
            
        case 0x7 :
            //description de Strategie n°5
            lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
            break;
            
        case 0x8 :
            //description de Strategie n°5
            lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
            break;
            
        case 0x9 :
            //description de Strategie n°5
            lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
            break;
            
        case 0xA :
            //description de Strategie n°5
            lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
            break;
        }
}

void Setflag(void)
{
    flagSendCan = 1;
}

void affichage_debug(E_stratGameEtat Var){
    int i;
    int conv=(int)Var;
    SUIVANT.Draw(ROUGE, 0);
    for(i=0;i<9;i++){
        strcpy(tableau_aff[i],"");
        strcpy(tableau_aff[i],tableau_aff[i+1]);
    }
    strcpy(tableau_aff[9],tableau_etat[conv]);
    for(i=0;i<10;i++){
        lcd.SetBackColor(BLEU);
        lcd.DisplayStringAt(0, LINE(20+i), (uint8_t *)tableau_aff[i], LEFT_MODE);
    }  
    while(!ack_bluetooth){
        liaison_bluetooth();
    }
    ack_bluetooth=0;
} 


void automate_etat_ihm(void)
{
    int j;
    if (j==0){
        ts.Init(lcd.GetXSize(), lcd.GetYSize());
        j++;
    }
    ts.GetState(&TS_State);       
    switch (etat)
    {
        case INIT :
            ts.GetState(&TS_State); 
            canProcessRx();
                
            lcd.SetBackColor(LCD_COLOR_WHITE);
            lcd.SetTextColor(LCD_COLOR_BLACK);
            lcd.Clear (LCD_COLOR_WHITE);
            wait(0.15);
            lcd.DisplayStringAt(0, 10, (uint8_t *)"Verification des cartes", LEFT_MODE);
            //cartes non verifiées////////////////
            lcd.SetTextColor(DIY_GREY);
            lcd.FillRect(0,400,400,150); //carte moteur
            lcd.FillRect(0,600,400,150); //Balise
            
            lcd.SetTextColor(LCD_COLOR_BLACK);
            lcd.SetBackColor(DIY_GREY);
            lcd.DisplayStringAt(80, 450, (uint8_t *)"Carte Moteur", LEFT_MODE);
            lcd.DisplayStringAt(110,650 , (uint8_t *)"Balise", LEFT_MODE);
            ////////////////////////////////////////
            
            FORCE_LAUNCH.Draw(0xFFFF0000, 0);

            /*while(flag == 0)
            {
                ts.GetState(&TS_State); 
                canProcessRx();
                if (FORCE_LAUNCH.Touched())
                {
                    etat = CHOIX;
                    gameEtat = ETAT_CONFIG;
                    flag = 1;
                    while(FORCE_LAUNCH.Touched());
                }
            }*/ 
            
            etat=ATT;
            break;
            
        case ATT :
            if (flag==1){
                etat = CHOIX;
                gameEtat = ETAT_CONFIG;
            }
            else if (FORCE_LAUNCH.Touched()){
                etat = CHOIX;
                gameEtat = ETAT_CONFIG;
            }
                
            break;
         
            
        case CHOIX :
            lcd.SetBackColor(LCD_COLOR_WHITE);
            lcd.SetTextColor(LCD_COLOR_BLACK);
            lcd.Clear (LCD_COLOR_WHITE);
            lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"Match ou demonstration ?", LEFT_MODE);
            DEMONSTRATION.Draw(LCD_COLOR_LIGHTGREEN, 0);
            MATCH.Draw(0xFFF01010, 0);
            while(etat == CHOIX)
            {
                canProcessRx();
                if(DEMONSTRATION.Touched())
                {
                    etat = DEMO;
                    while(DEMONSTRATION.Touched());
                }
                
                if(MATCH.Touched())
                {
                    etat = SELECT_SIDE;
                    while(MATCH.Touched());
                }

            }
            break;
        
        case DEMO :                                  
            lcd.Clear(LCD_COLOR_WHITE);

            TEST_MOTEUR.Draw(0xFFF0F0F0, 0);
            TEST_BRAS.Draw(0xFFF0F0F0, 0);
            RETOUR.Draw(0xFFFF0000, 0);
            if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config                                     //
                InversStrat = 0;//Pas d'inversion de la couleur                                 // A changer , discussion avec l'ihm
            }
            while (etat == DEMO)
            {
                canProcessRx();
                if(TEST_MOTEUR.Touched())
                {
                    Strat = 0x10;
                    while(TEST_MOTEUR.Touched());
                    TEST_MOTEUR.Draw(0xFFF0F0F0, 0);
                    etat = DEMO_EN_COURS;
                }
                
                if(TEST_BRAS.Touched())
                {
                    Strat = 0x11;                        
                    while(TEST_BRAS.Touched());                 
                    TEST_MOTEUR.Draw(0xFFF0F0F0, 0);
                    etat = DEMO_EN_COURS;
                }
                
                if(RETOUR.Touched())
                {
                    etat = CHOIX;
                    while(RETOUR.Touched());
                }
                if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config
                    if (Strat< 0x10){  // Si la strat est une strat de match, on desactive le mode demo
                            ModeDemo = 0;
                    }
                    else {                                        // sinon, on active le mode demo, utile pour la fin de la demo
                            ModeDemo = 1;
                    }
                    
                    Ack_strat = 1;
                    wait_ms(10);
                    tactile_printf("Strat %d, Asser desactive",Strat);
                }
                SelectionStrat(Strategie);
            }
            break;
            
        case DEMO_EN_COURS:
            lcd.Clear(LCD_COLOR_WHITE);
            lcd.SetBackColor(LCD_COLOR_WHITE);
            lcd.DisplayStringAt(20, LINE(10), (uint8_t *)"DEMONSTRATION EN COURS", LEFT_MODE);
            
            
            while (etat == DEMO_EN_COURS) 
            {
                canProcessRx();
                
                if (Ack_strat == 1)
                {
                    if(gameEtat == ETAT_CONFIG) {
                        gameEtat = ETAT_GAME_INIT;
                    }              
                    while (flag_strat != 1)
                    {
                      canProcessRx();
                    }
                    etat = DEMO;
                    flag_strat = 0;  
                }
                  
            }
            Ack_strat = 0;
            break;
            
        case SELECT_SIDE :
            lcd.Clear(LCD_COLOR_WHITE);
            lcd.SetBackColor(LCD_COLOR_WHITE);
            lcd.SetTextColor(LCD_COLOR_BLACK);
             
            lcd.DisplayStringAt(70, LINE(0), (uint8_t *)"Choisir le cote", LEFT_MODE);
            COTE_BLEU.Draw(BLEU, 0);
            COTE_JAUNE.Draw(JAUNE, 0);
            RETOUR.Draw(LCD_COLOR_RED, 0);
            
                
            while (etat == SELECT_SIDE) 
            {
                canProcessRx();
                if(COTE_BLEU.Touched()) 
                {
                    Cote = 0x0;
                    InversStrat = Cote;
                    etat = TACTIQUE;
                    while(COTE_BLEU.Touched());
                    envoi_bluetooth(1,'9');
                }
                
                if(COTE_JAUNE.Touched())
                {
                    Cote = 0x1;
                    InversStrat= Cote;
                    etat = TACTIQUE;
                    while(COTE_JAUNE.Touched());
                }
                
                if(RETOUR.Touched())
                {
                    etat = CHOIX;
                    while(RETOUR.Touched());
                }
            }
            
            break;
                
        case TACTIQUE :
            if (Cote == 0){
                lcd.Clear(BLEU);
                lcd.SetBackColor(BLEU);
                }
            else if (Cote == 1){
                lcd.Clear(JAUNE);
                lcd.SetBackColor(JAUNE);
                }
            else {
                lcd.Clear(BLEU);
                lcd.SetBackColor(BLEU);
                }
            
            lcd.SetTextColor(LCD_COLOR_BLACK); 
                
            lcd.DisplayStringAt(20, LINE(0), (uint8_t *)"Choisir une strategie", LEFT_MODE);
            
            Strategie = Bouton_Strat(); // retourne valeur de Strategie si bouton strat renvoi -1 on reviens en arriere
            if (Strategie == -1) 
            {
                etat = SELECT_SIDE;
            }
            else 
            {
                etat = DETAILS;
            }  
            wait(0.1);
            break;
        
        case DETAILS :
            lcd.Clear(LCD_COLOR_WHITE);
            lcd.SetBackColor(LCD_COLOR_WHITE);
            lcd.SetTextColor(LCD_COLOR_BLACK);
            CHECK.Draw(VERT);
            RETOUR.Draw(LCD_COLOR_RED);
            
            SelectionStrat(Strategie);
             
            while (etat == DETAILS)
            { 
                canProcessRx();
                if (CHECK.Touched())
                    {
                        if(gameEtat == ETAT_CONFIG) {
                            gameEtat = ETAT_GAME_INIT;
                            etat=LECTURE;
                            
                        }
                        while(CHECK.Touched());
                    }
                
                if(RETOUR.Touched())
                    {
                        etat = TACTIQUE;
                        while(RETOUR.Touched());
                    }
            } 
            break;
        
        
        case LECTURE :
            break;   
        case AFF_WAIT_JACK : 
            lcd.Clear(BLANC);
            lcd.SetBackColor(LCD_COLOR_WHITE);
            lcd.SetTextColor(LCD_COLOR_BLACK);
             
            if (Cote == 0){
                lcd.Clear(BLEU);
                lcd.SetBackColor(BLEU);
                }
            else if (Cote == 1){
                lcd.Clear(JAUNE);
                lcd.SetBackColor(JAUNE);
                }
            else {
                lcd.Clear(BLEU);
                lcd.SetBackColor(BLEU);
            }
            canProcessRx();
            lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"En attente du Jack", CENTER_MODE); 
            etat=WAIT_JACK;
            break;
            
        case WAIT_JACK:
            break;   
            
        case COMPTEUR: 
            cptf=gameTimer.read();
            lcd.SetTextColor(LCD_COLOR_BLACK);
            cpt=int(cptf);
            if(cpt != cpt1){
                lcd.Clear(BLEU);
                affichage_compteur(90-cpt);
            }
            cpt1=cpt;
            flag_timer=0;

            affichage_debug(gameEtat);
            
            
            break;
            
        case FIN :
            lcd.Clear (LCD_COLOR_WHITE);
            lcd.SetBackColor(LCD_COLOR_WHITE);
            lcd.DisplayStringAt(120, 190, (uint8_t *)"LANCEMENT", LEFT_MODE);
            lcd.SetTextColor(LCD_COLOR_BLACK);
            lcd.DisplayStringAt(120, 210, (uint8_t *)"REDEMARAGE", LEFT_MODE);
            lcd.DisplayStringAt(120, 230, (uint8_t *)"NECESSAIRE", LEFT_MODE);
            while(1); // force le redemarage du robot  
            //break;
            
    }    
}



/****************************************************************************************/
/* FUNCTION NAME: automate_process                                                      */
/* DESCRIPTION  : Automate de gestion de la stratégie du robot                          */
/****************************************************************************************/
void automate_process(void){
    static unsigned char AX12_enchainement = 0;
    static unsigned char MV_enchainement = 0;
    signed char localData1 = 0;
    signed short localData2 = 0;
    unsigned short localData3 = 0;
    //signed short localData4 = 0;
    unsigned char localData5 = 0;
    
    if(gameTimer.read_ms() >= 89000) {//Fin du match (On autorise 2s pour déposer des éléments
        gameTimer.stop();
        gameTimer.reset();
        gameEtat = ETAT_END;//Fin du temps
        etat=FIN;
    }
    
    if(lastEtat != gameEtat || debugetatTimer.read_ms() >= 1000) {
        lastEtat = gameEtat;
        debugetatTimer.reset();
        sendStratEtat((unsigned char)gameEtat, (unsigned char)actual_instruction);
    }
    
    switch(gameEtat)
    {
        case ETAT_CHECK_CARTES:                   
            /*
            Il faut faire une boucle pour verifier toutes les cartes les une apres les autres
            */
            waitingAckFrom = id_alive[checkCurrent];//On indique que l'on attend un ack de la carte IHM
            SendRawId(id_check[checkCurrent]);//On demande à la carte d'indiquer ça présence
            
            screenChecktry++;//On incrèment le conteur de tentative de 1
            cartesCheker.reset();//On reset le timeOut
            cartesCheker.start();//On lance le timer pour le timeout
            gameEtat = ETAT_CHECK_CARTES_WAIT_ACK;
            break;
            
        case ETAT_CHECK_CARTES_WAIT_ACK:
            /*
            On attend l'ack de la carte en cours de vérification
            */
            //printf("cartesCheker = %d waitingAckFrom = %d\n",cartesCheker.read_ms(), waitingAckFrom);
            if(waitingAckFrom == 0) {//C'est bon la carte est en ligne
                cartesCheker.stop();
                screenChecktry = 0;
                countAliveCard++;
                checkCurrent++;
                if(checkCurrent >= NOMBRE_CARTES) {
                    printf("all card check, missing %d cards\n",(NOMBRE_CARTES-countAliveCard));
                    if(countAliveCard >= NOMBRE_CARTES) {
                        gameEtat = ETAT_CONFIG;
                        flag=1;
                        tactile_printf("Selection couleur et strategie");
                    } 
                    else {
                        gameEtat = ETAT_WAIT_FORCE;//Passage en attente de forçage du lancement
                        waitingAckFrom = ECRAN_ALL_CHECK;
                    }
                } 
                else
                    gameEtat = ETAT_CHECK_CARTES;
            } 
            else if(cartesCheker.read_ms () > 100) {
                cartesCheker.stop();
                if(screenChecktry >=3) {
                    //printf("missing card %d\n",id_check[checkCurrent]);
                    screenChecktry = 0;
                    checkCurrent++;
                    
                    if(checkCurrent >= NOMBRE_CARTES){
                        if(countAliveCard == NOMBRE_CARTES){
                            gameEtat = ETAT_CONFIG;
                            flag=1; 
                        } 
                        else{
                            gameEtat = ETAT_WAIT_FORCE;
                            waitingAckFrom = ECRAN_ALL_CHECK;
                        }
                    } 
                    else
                        gameEtat = ETAT_CHECK_CARTES;
                    
                } 
                else
                    gameEtat = ETAT_CHECK_CARTES;
                
            }
            break;
        case ETAT_WAIT_FORCE:
            /*
            Attente du forçage de la part de la carte IHM
            */
            if(waitingAckFrom == 0) {
                gameEtat = ETAT_CONFIG;
            }
            break;
        case ETAT_CONFIG:
            /*
            Attente de l'odre de choix de mode,
            Il est possible de modifier la couleur et l'id de la stratégie
            Il est aussi possible d'envoyer les ordres de debug
            */
            modeTelemetre = 0;
            break;
        case ETAT_GAME_INIT:
            //On charge la liste des instructions
            
            loadAllInstruction(Strategie);//Mise en cache de toute les instructions
            led3=1;
            
            gameEtat = ETAT_GAME_WAIT_FOR_JACK;
            if (etat == DEMO_EN_COURS) 
            {
                SendRawId(DEBUG_FAKE_JAKE);
            }
            else
            {
                etat = AFF_WAIT_JACK;
            }                          
            tactile_printf("Attente du JACK.");
            setAsservissementEtat(1);//On réactive l'asservissement
            jack.mode(PullDown); // désactivation de la résistance interne du jack
            jack.fall(&jack_ISR); // création de l'interrupt attachée au changement d'état (front descendant) sur le jack
         
#ifdef ROBOT_BIG //le gros robot n'a pas de recalage bordure pour ce placer au début, on lui envoit donc ça position
            localData2 = POSITION_DEBUT_T;
            localData3 = POSITION_DEBUT_Y;
            if(InversStrat == 1) {
                localData2 = 1800-localData2;//Inversion theta
                if(localData2 > 1800) localData2 -= 3600;
                else if(localData2 <= -1800) localData2 += 3600;
                localData3 = 3000 - POSITION_DEBUT_Y;//Inversion du Y
            }
            SetOdometrie(ODOMETRIE_BIG_POSITION, POSITION_DEBUT_X,localData3,localData2);
#endif 
#ifdef ROBOT_SMALL
            localData2 = POSITION_DEBUT_T;
            localData3 = POSITION_DEBUT_Y;
            if(InversStrat == 1) {
                localData2 = -localData2;//Inversion theta
                localData3 = 3000 - POSITION_DEBUT_Y;//Inversion du Y
            }
            SetOdometrie(ODOMETRIE_SMALL_POSITION, POSITION_DEBUT_X,localData3,localData2);
#endif
        break;
        case ETAT_GAME_WAIT_FOR_JACK:
            //On attend le jack
        break;
        case ETAT_GAME_START:
            
            gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
            
            if (ModeDemo == 0){
                chronoEnd.attach(&chronometre_ISR,90);//On lance le chrono de 90s
                gameTimer.start();
            } 
            gameTimer.reset();
            jack.fall(NULL);//On désactive l'interruption du jack
            SendRawId(GLOBAL_START);  
            Jack=0;                                          //à envoyer sur le CAN et en direct pour l'automate de l'ihm ou sur CANV
            tactile_printf("Start");//Pas vraiment utile mais bon
        break;
        case ETAT_GAME_LOAD_NEXT_INSTRUCTION:
            /*
            Chargement de l'instruction suivante ou arret du robot si il n'y a plus d'instruction
            */
            //printf("load next instruction\n");
            
            if(actual_instruction >= nb_instructions || actual_instruction == 255) {
                gameEtat = ETAT_END;
                //Il n'y a plus d'instruction, fin du jeu
            } else {
                instruction = strat_instructions[actual_instruction];
                //On effectue le traitement de l'instruction 
                gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;
            }
            screenChecktry = 0;
            ingnorInversionOnce = 0;
        break;
        case ETAT_GAME_PROCESS_INSTRUCTION:
            /*
            Traitement de l'instruction, envoie de la trame CAN
            */
            //debug_Instruction(instruction);
            
            actionPrecedente = instruction.order;
            switch(instruction.order)
            {
                case MV_COURBURE://C'est un rayon de courbure
                    actionPrecedente = MV_COURBURE;
                    waitingAckID = ASSERVISSEMENT_COURBURE;
                    waitingAckFrom = ACKNOWLEDGE_MOTEUR;
                    if(instruction.nextActionType == ENCHAINEMENT) {
                        MV_enchainement++;
                        localData5 = 1;
                    } else {
                        if(MV_enchainement > 0) {
                            localData5 = 2;
                            MV_enchainement = 0;
                        } else {
                            localData5 = 0;
                        }
                    }
                    localData1 = ((instruction.direction == LEFT)?1:-1);
                    localData2 =  instruction.arg3;
                    if(InversStrat == 1 && ingnorInversionOnce == 0)
                    {
                        localData1 = -localData1;//Inversion de la direction
                        #ifdef ROBOT_BIG
                        localData2 = -localData2;
                        localData1 = -localData1;//Inversion de la direction
                        #endif
                    }
                    BendRadius(instruction.arg1, localData2, localData1, localData5);
                    
                    
                    target_theta_robot =  localData2 - theta_robot;
                    /*
                    if(instruction.direction == LEFT){
                        
                    }else{
                        target_theta_robot = theta_robot + localData2;
                        }*/
                    
                break;
                case MV_LINE://Ligne droite
                    waitingAckID = ASSERVISSEMENT_RECALAGE;
                    waitingAckFrom = ACKNOWLEDGE_MOTEUR;
                    if(instruction.nextActionType == ENCHAINEMENT) {
                        MV_enchainement++;
                        localData5 = 1;
                    } else {
                        if(MV_enchainement > 0) {//Utilisé en cas d'enchainement, 
                            localData5 = 2;
                            MV_enchainement = 0;
                        } else {
                            localData5 = 0;
                        }
                    }
                    #ifdef ROBOT_BIG
                    if(InversStrat == 1 && ingnorInversionOnce == 0) {
                            /*if (instruction.direction == FORWARD) instruction.direction = BACKWARD;
                            else instruction.direction = FORWARD;*/
                            instruction.direction = ((instruction.direction == FORWARD)?BACKWARD:FORWARD);
                        }
                    #endif    
                    localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1);
                    GoStraight(localData2, 0, 0, localData5);
                    
                    target_x_robot = x_robot + localData2*cos((double)theta_robot*M_PI/1800); 
                    target_y_robot = y_robot + localData2*sin((double)theta_robot*M_PI/1800);
                    target_theta_robot = theta_robot;
                    
                break;
                case MV_TURN: //Rotation sur place
                    if(instruction.direction == RELATIVE) {
                        localData2 = instruction.arg3;
                    } else {//C'est un rotation absolu, il faut la convertir en relative
                        localData2 = instruction.arg3;
                        
                        localData2 = (localData2 - theta_robot)%3600;
                        if(localData2 > 1800) {
                            localData2 = localData2-3600;
                        }
                        
                    }
                    #ifdef ROBOT_SMALL
                    if(InversStrat == 1 && ingnorInversionOnce == 0) {
                            localData2 = -localData2;
                        }
                    #endif
                    #ifdef ROBOT_BIG
                    if(InversStrat == 1 && ingnorInversionOnce == 0) {
                        if(instruction.direction == RELATIVE) {
                            localData2 = instruction.arg3;
                        } else {//C'est un rotation absolu, il faut la convertir en relative
                            
                            localData2 = 1800-instruction.arg3;//Inversion theta
                            if(localData2 > 1800) localData2 -= 3600;
                            else if(localData2 <= -1800) localData2 += 3600;
                            
                            localData2 = (localData2 - theta_robot)%3600;
                            if(localData2 > 1800) {
                                localData2 = localData2-3600;
                            }
                            
                        }
                    }
                    #endif  
                    Rotate(localData2);
                    waitingAckID = ASSERVISSEMENT_ROTATION;
                    waitingAckFrom = ACKNOWLEDGE_MOTEUR;
                      
                break;
                case MV_XYT:
                    if(instruction.direction == BACKWARD) {
                        localData1 = -1;
                    } else {
                        localData1 = 1;
                    }
                    
                    if(InversStrat == 1 && ingnorInversionOnce == 0) {
                        localData2 = -instruction.arg3;
                        localData3 = 3000 - instruction.arg2;//Inversion du Y
                    } else {
                        localData3 = instruction.arg2;
                        localData2 = instruction.arg3;
                    }
                    
                    #ifdef ROBOT_BIG
                    if(InversStrat == 1 && ingnorInversionOnce == 0) {
                         localData1 = -localData1;
                         localData2 = 1800-instruction.arg3;//Inversion theta
                         if(localData2 > 1800) localData2 -= 3600;
                         else if(localData2 <= -1800) localData2 += 3600;
                    }
                    #endif
                    
                    GoToPosition(instruction.arg1,localData3,localData2,localData1);
                    waitingAckID = ASSERVISSEMENT_XYT;
                    waitingAckFrom = ACKNOWLEDGE_MOTEUR;
                    
                    target_x_robot = instruction.arg1; 
                    target_y_robot = localData3;
                    target_theta_robot = localData2;
                    
                break;
                case MV_RECALAGE:
                    waitingAckID = ASSERVISSEMENT_RECALAGE;
                    waitingAckFrom = ACKNOWLEDGE_MOTEUR;
                    instruction.nextActionType = WAIT;
                    localData2 = (((instruction.direction == FORWARD)?1:-1)*3000);//On indique une distance de 3000 pour etre sur que le robot va ce recaler
                    
                    if(instruction.precision == RECALAGE_Y) {
                        localData5 = 2;
                        if(InversStrat == 1 && ingnorInversionOnce == 0) {
                            localData3 = 3000 - instruction.arg1;//Inversion du Y
                        } else {
                            localData3 = instruction.arg1;
                        }
                    } else {
                        localData5 = 1;
                        localData3 = instruction.arg1;
                    }
                    
                    GoStraight(localData2, localData5, localData3, 0);
                break;
                case ACTION:
                    int tempo = 0;
                    waitingAckID= SERVO_AX12_ACTION;
                    waitingAckFrom = ACKNOWLEDGE_AX12;
                    tempo = doAction(instruction.arg1,instruction.arg2,instruction.arg3);
                    if(tempo == 1){
                        //L'action est spécifique
                        if((waitingAckFrom == 0 && waitingAckID == 0) || instruction.nextActionType == ENCHAINEMENT) {
                            
                            actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
                            gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
                        } else {
                            gameEtat = ETAT_GAME_WAIT_ACK;
                        }
                        #ifdef ROBOT_SMALL
                            actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
                            gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
                        #endif
                        return;
                     #ifdef ROBOT_SMALL   
                    } else if (tempo == 2) {
                        // on est dans le cas de l'avance selon le telemetre
                        waitingAckID = ASSERVISSEMENT_RECALAGE;
                        waitingAckFrom = ACKNOWLEDGE_MOTEUR;
                        
                        localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1);
                        GoStraight(telemetreDistance, 0, 0, 0);
                        // on reset la distance du telemetre à 0
                        telemetreDistance = 5000;
                        #endif
                    }else{
                        //C'est un AX12 qu'il faut bouger
                        //AX12_setGoal(instruction.arg1,instruction.arg3/10,instruction.arg2);
                        //AX12_enchainement++;
                                                
                    }
                break;
                default:
                    //Instruction inconnue, on l'ignore
                break;
            }    
            
            
            
            if(instruction.nextActionType == JUMP || instruction.nextActionType == WAIT) {
                gameEtat = ETAT_GAME_WAIT_ACK;//Il faut attendre que la carte est bien reçu l'acknowledge
                screenChecktry++;//On incrèment le conteur de tentative de 1
                cartesCheker.reset();//On reset le timeOut
                cartesCheker.start();
                if(AX12_enchainement > 0) {
                    //AX12_processChange();//Il faut lancer le déplacement des AX12
                    //AX12_enchainement = 0;
                }
            } else {//C'est un enchainement
                if(instruction.order == MV_LINE){
                      gameEtat =  ETAT_GAME_WAIT_ACK;
                    
                }else{
                    actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
                    gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//C'est un enchainement, on charge directement l'instruction suivante
                }
            }
            
        break;
        case ETAT_GAME_WAIT_ACK:
            /*
            Attente de l'ack de l'instruction
            */
            if(waitingAckID == 0 && waitingAckFrom == 0) {//Les ack ont été reset, c'est bon on continu
            //if(true) {
                cartesCheker.stop();
                if(instruction.nextActionType == JUMP) {
                    if(instruction.jumpAction == JUMP_POSITION) {
                        gameEtat = ETAT_GAME_JUMP_POSITION;
                    } else {//Pour eviter les erreurs, on dit que c'est par défaut un jump time
                        gameEtat = ETAT_GAME_JUMP_TIME;
                        cartesCheker.reset();//On reset le timeOut
                        cartesCheker.start();  
                    }
                } else if(instruction.nextActionType == WAIT) {
                    gameEtat = ETAT_GAME_WAIT_END_INSTRUCTION;
                     switch(instruction.order)
                    {
                        case MV_COURBURE:
                            waitingAckID = ASSERVISSEMENT_COURBURE;
                            waitingAckFrom = INSTRUCTION_END_MOTEUR;
                        break;
                        case MV_LINE:
                            waitingAckID = ASSERVISSEMENT_RECALAGE;
                            waitingAckFrom = INSTRUCTION_END_MOTEUR;
                        break;
                        case MV_TURN:
                            waitingAckID = ASSERVISSEMENT_ROTATION;
                            waitingAckFrom = INSTRUCTION_END_MOTEUR;
                        break;
                        case MV_XYT:
                            waitingAckID = ASSERVISSEMENT_XYT;
                            waitingAckFrom = INSTRUCTION_END_MOTEUR;
                        break;
                        case MV_RECALAGE:
                            waitingAckID = ASSERVISSEMENT_RECALAGE;
                            waitingAckFrom = INSTRUCTION_END_MOTEUR;
                        break;
                        case ACTION:
                            
                            if (modeTelemetre == 0){
                                if (telemetreDistance == 0){
                                    waitingAckID = SERVO_AX12_ACTION;// instruction.arg1;  
                                    waitingAckFrom = INSTRUCTION_END_AX12; //SERVO_AX12_DONE;
                                }else if(telemetreDistance == 5000){
                                    // on est dans le cas ou l'on fait une ligne suivant la distance du telemetre
                                    waitingAckID = ASSERVISSEMENT_RECALAGE;
                                    waitingAckFrom = INSTRUCTION_END_MOTEUR;
                                    telemetreDistance = 0;
                                }
                            }else{ // si on attend la reponse du telemetre  
                                //modeTelemetre = 1; 
                                waitingAckID = OBJET_SUR_TABLE;
                                waitingAckFrom = 0; 
                            }
                        break;
                        default:
                        break;
                    }   
                } else {
                    gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
                    actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
                }
            } else if(cartesCheker.read_ms () > 50){
                cartesCheker.stop();
                if(screenChecktry >=2) {//La carte n'a pas reçus l'information, on passe à l'instruction d'erreur
                    actual_instruction = instruction.nextLineError;
                    gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
                } else {
                    gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;//On retourne dans l'etat d'envois de l'instruction
                }
            }
        break;
        
        case ETAT_GAME_JUMP_TIME:
            if(cartesCheker.read_ms () >= instruction.JumpTimeOrX) {
                cartesCheker.stop();//On arrete le timer
                actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
                gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante
            }
        break;
        
        case ETAT_GAME_JUMP_CONFIG:
            signed int depasX = 1, depasY = 1;  // servent à indiquer le sens de dépassement des coordonnées
                                                //  1 si l'instruction est plus grande que la position du robot
                                                // -1 si l'instruction est plus petite que la position du robot
                                                //  0 si l'instruction et position du robot sont proche de moins de 1cm
            if (abs(x_robot-instruction.JumpTimeOrX)<10){
                    depasX = 0;
            }else if(x_robot > instruction.JumpTimeOrX){
                    depasX = -1;
            }
            
            if(abs(y_robot-instruction.JumpY)<10){
                    depasY = 0;
            }else if(y_robot > instruction.JumpY){
                    depasY = -1;
            }
                
            gameEtat = ETAT_GAME_JUMP_POSITION;
        break;
        case ETAT_GAME_JUMP_POSITION:
            bool Xok = false, Yok = false;
             
                if (depasX == 0){
                    Xok = true;    
                }else if ((instruction.JumpTimeOrX - x_robot)*depasX < -5){
                    Xok = true;
                }
                
                if (depasY == 0){
                    Yok = true;    
                }else if ((instruction.JumpY - y_robot)*depasY < -5){
                    Yok = true;
                }
                
                // on teste si les deux coordonnées ont été dépassées, si oui on lance l'instruction suivante
                if (Xok && Yok){
                        actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
                        gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante
                    }
                
        break;
        case ETAT_GAME_WAIT_END_INSTRUCTION:
            if(waitingAckID == 0 && waitingAckFrom ==0) {//On attend que la carte nous indique que l'instruction est terminée
                actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
                gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante
            }
        break;
        
        
        case ETAT_WARNING_TIMEOUT://Attente de la trame fin de danger ou du timeout de 2s
            if(timeoutWarning.read_ms() >= BALISE_TIMEOUT)//ça fait plus de 2s, il faut changer de stratégie
            {
                gameEtat = ETAT_WARNING_SWITCH_STRATEGIE;
            }
        break;
        case ETAT_WARING_END_BALISE_WAIT://Attente d'une seconde apres la fin d'un End Balise pour etre sur que c'est bon
            if(timeoutWarningWaitEnd.read_ms() >= 1000) {//c'est bon, on repart
                //actual_instruction = instruction.nextLineError;
                gameEtat = ETAT_WARNING_END_LAST_INSTRUCTION;
            }
        break;
        case ETAT_WARNING_END_LAST_INSTRUCTION://trouver le meilleur moyen de reprendre l'instruction en cours
/*
#ifdef ROBOT_BIG
            actual_instruction = instruction.nextLineError;//  2 //Modification directe... c'est pas bien mais ça marchait pour le match 5
            gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
#else       
            actual_instruction = instruction.nextLineError;
            gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; 
#endif      
            gameEtat = ETAT_END;*/
            
            switch(actionPrecedente){
                case MV_LINE:
                
                    if(instruction.direction == BACKWARD) {
                        localData1 = -1;
                    } else {
                        localData1 = 1;
                    }
                    
                    GoToPosition(target_x_robot,target_y_robot,target_theta_robot,localData1);
                    debugXYTTarget(target_x_robot,target_y_robot,target_theta_robot);
                    waitingAckID = ASSERVISSEMENT_XYT;
                    waitingAckFrom = ACKNOWLEDGE_MOTEUR;
                    gameEtat = ETAT_GAME_WAIT_ACK; 
                    instruction.order = MV_XYT;
                    instruction.arg1 = target_x_robot;
                    instruction.arg2 = target_y_robot;
                    instruction.arg3 = target_theta_robot;
                    instruction.direction = (localData1)?FORWARD:BACKWARD;
                    ingnorInversionOnce = 1;//Pour éviter que l'ago recalcul l'inversion
                    return;
                    
                case MV_XYT:
                    gameEtat = ETAT_GAME_PROCESS_INSTRUCTION; 
                    break;
                    
                case MV_COURBURE:
                    //target_theta_robot = theta_robot - target_theta_robot;
                    //instruction.arg3 =  instruction.arg3 - target_theta_robot;
                    if(instruction.direction == LEFT){
                        target_theta_robot = target_theta_robot - theta_robot;
                    }else{
                        target_theta_robot = theta_robot + target_theta_robot;
                    }
                    
                    
                    target_theta_robot = (target_theta_robot)%3600;
                    if(target_theta_robot > 1800) {
                        target_theta_robot = target_theta_robot-3600;
                    }
                     if(InversStrat == 1) {
                        target_theta_robot = -target_theta_robot;
                    }
                    instruction.arg3 = target_theta_robot;
                    
                    
                    gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;                     
                    
                    break;
                default:
                    actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
                    gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
                break;
            }
            
            //actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
            //gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
        break;
        case ETAT_WARNING_SWITCH_STRATEGIE://Si à la fin du timeout il y a toujours un robot, passer à l'instruction d'erreur
            actual_instruction = instruction.nextLineError;
            gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
            ingnorBaliseOnce = 1;
        break;
            
            
            
        case ETAT_END: 
            if (ModeDemo){
                gameEtat = ETAT_CHECK_CARTE_SCREEN;
                ModeDemo = 1;
            } else {
                gameEtat = ETAT_END_LOOP;
            }
        break;
        case ETAT_END_LOOP:
            //Rien, on tourne en rond
            
        break;
        default:
            
        break;
    }       
}          





/****************************************************************************************/
/* FUNCTION NAME: canProcessRx                                                          */
/* DESCRIPTION  : Fait évoluer l'automate de l'IHM en fonction des receptions sur le CAN*/
/****************************************************************************************/
void canProcessRx(void)
{
    static signed char FIFO_occupation=0,FIFO_max_occupation=0;
    char useless1 = 0;

    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 ECRAN_PRINTF_1:
                for(useless1 = 0; useless1 <8; useless1++) {
                    test[useless1] = msgRxBuffer[FIFO_lecture].data[useless1];
                }
                break;
            case ECRAN_PRINTF_2:
                for(useless1 = 0; useless1 <8; useless1++) {
                    test[useless1+8] = msgRxBuffer[FIFO_lecture].data[useless1];
                }
                break;
            case ECRAN_PRINTF_3:
                for(useless1 = 0; useless1 <8; useless1++) {
                    test[useless1+2*8] = msgRxBuffer[FIFO_lecture].data[useless1];
                }
                break;
            case ECRAN_PRINTF_4:
                for(useless1 = 0; useless1 <8; useless1++) {
                    test[useless1+3*8] = msgRxBuffer[FIFO_lecture].data[useless1];
                }
                break;
            case ECRAN_PRINTF_CLEAR:
                etat = CHOIX;
                break;
                
            case ALIVE_MOTEUR:
            if (etat == ATT) {
                 
                lcd.SetTextColor(LCD_COLOR_LIGHTGREEN);
                lcd.FillRect(0,75,400,150); //carte moteur
                lcd.SetTextColor(LCD_COLOR_BLACK);
                lcd.SetBackColor(LCD_COLOR_LIGHTGREEN);
                lcd.DisplayStringAt(80, 135, (uint8_t *)"Carte Moteur", LEFT_MODE);
                }
                break;
                
            case ALIVE_BALISE:
            if (etat == ATT) {
                 
                lcd.SetTextColor(LCD_COLOR_LIGHTGREEN);
                lcd.FillRect(0,250,400,150); //carte AX12
                lcd.SetTextColor(LCD_COLOR_BLACK);
                lcd.SetBackColor(LCD_COLOR_LIGHTGREEN);
                lcd.DisplayStringAt(110, 310, (uint8_t *)"Balise", LEFT_MODE);
                } 
                break;

            case RESET_IHM:
                etat = CHOIX;
                break;

            case DEBUG_FAKE_JAKE://Permet de lancer le match à distance
            case GLOBAL_JACK:
                if(gameEtat == ETAT_GAME_WAIT_FOR_JACK) {
                    gameEtat = ETAT_GAME_START;
                    SendRawId(ACKNOWLEDGE_JACK);
                }
            break;
            
            case ALIVE_ACTIONNEURS_AVANT:
            case ALIVE_ACTIONNEURS_ARRIERE:
            case ALIVE_AX12:
            case ECRAN_ALL_CHECK:
                if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id) {
                    waitingAckFrom = 0;//C'est la bonne carte qui indique qu'elle est en ligne
                }
            break; 
            
            case ACKNOWLEDGE_BALISE:
            case ACKNOWLEDGE_MOTEUR:
            case ACKNOWLEDGE_TELEMETRE:
            case ACKNOWLEDGE_AX12:
            case INSTRUCTION_END_BALISE:
            case INSTRUCTION_END_MOTEUR:
            case INSTRUCTION_END_IHM:
            case INSTRUCTION_END_AX12:
                
                if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id && ((unsigned short)msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8) == waitingAckID)) {
                    waitingAckFrom = 0;
                    waitingAckID = 0;
                }
            break;
#ifdef ROBOT_BIG
            case ODOMETRIE_BIG_POSITION:
#else
            case ODOMETRIE_SMALL_POSITION:
#endif
                x_robot=msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8);
                y_robot=msgRxBuffer[FIFO_lecture].data[2]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[3])<<8);
                theta_robot=msgRxBuffer[FIFO_lecture].data[4]|((signed short)(msgRxBuffer[FIFO_lecture].data[5])<<8);
            break;
    
            case SERVO_AX12_SETGOAL:
                //SendAck(0x114, SERVO_AX12_SETGOAL);
                //if(AX12_isLocal(msgRxBuffer[FIFO_lecture].data[0]))
                    //AX12_setGoal(msgRxBuffer[FIFO_lecture].data[0], msgRxBuffer[FIFO_lecture].data[1]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[2])<<8), msgRxBuffer[FIFO_lecture].data[3]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[4])<<8));
                  
            break;
            
            case SERVO_AX12_PROCESS:
                SendAck(0x114, SERVO_AX12_PROCESS);
                //AX12_processChange(1);
            break;
            
            case SERVO_AX12_DONE:
                SendRawId(POMPE_PWM);
                /*//SendAck(0x114, SERVO_AX12_DONE);
                AX12_notifyCANEnd(((unsigned short)(msgRxBuffer[FIFO_lecture].data[0])));
                
                gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
                waitingAckFrom = 0;
                waitingAckID = 0;*/
                
            break;
    
            case BALISE_DANGER :
                SendAck(ACKNOWLEDGE_BALISE, BALISE_END_DANGER);
            break;
            
            case BALISE_STOP:
                SendAck(ACKNOWLEDGE_BALISE, BALISE_STOP);
                if (instruction.order != MV_TURN && instruction.order != ACTION ){ //J'ai rajouté cette ligne mais il faut tester avec et sans pour voir le comportement du robot, 
                    if(needToStop() != 0 && ingnorBaliseOnce ==0) {
                        if(gameEtat > ETAT_GAME_START && gameEtat != ETAT_WARNING_TIMEOUT)
                        {
                            SendRawId(ASSERVISSEMENT_STOP);
                            //while(1); // ligne à décommenter si on est en homologation
                            if(gameEtat != ETAT_WARING_END_BALISE_WAIT) {
                                timeoutWarning.reset();
                                timeoutWarning.start();//Reset du timer utiliser par le timeout
                            }
                            gameEtat = ETAT_WARNING_TIMEOUT;
                        }
                    }
                }
                ingnorBaliseOnce = 0;
            break;
            
            case BALISE_END_DANGER:
                SendAck(ACKNOWLEDGE_BALISE, BALISE_END_DANGER);
                if(gameEtat == ETAT_WARNING_TIMEOUT) {
                    timeoutWarningWaitEnd.reset();
                    timeoutWarningWaitEnd.start();
                    gameEtat = ETAT_WARING_END_BALISE_WAIT;
                }
            break;
            
            case ECRAN_CHOICE_START_ACTION:
                if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config
                    if(msgRxBuffer[FIFO_lecture].data[0] == 1) {
                        runRobotTest();
                    } else {
                        initRobotActionneur();
                    }
                    wait_ms(500);
                    SendRawId(ECRAN_ACK_CHOICE_START_ACTION);
                }
            break;
            
            case OBJET_SUR_TABLE:
                if (msgRxBuffer[FIFO_lecture].data[1] == 0xff){
                        
                        gameEtat = ETAT_WARNING_END_LAST_INSTRUCTION;
                    }
                else{
                        
                        waitingAckFrom = 0;
                        waitingAckID = 0; 
                        
                        strat_instructions[actual_instruction+1].arg1 = returnX(strat_instructions[actual_instruction].arg2);
                        strat_instructions[actual_instruction+1].arg2 = returnY(strat_instructions[actual_instruction].arg2);
                    }
                modeTelemetre = 0;
            break;
            
            
            
            
        }
        FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
    }
}





/****************************************************************************************/
/* FUNCTION NAME: Bouton_Strat                                                          */
/* DESCRIPTION  : Sélection de la strat sur le lcd puis envoie sur CAN (à modifier!)    */
/****************************************************************************************/
signed char Bouton_Strat (void)
{ 
    Button STRAT_1 (0, 30, 190, 110, strat_sd[0]);
    Button STRAT_2 (210, 30, 190, 110, strat_sd[1]);
    Button STRAT_3 (0, 150, 190, 110, strat_sd[2]);
    Button STRAT_4 (210, 150, 190, 110, strat_sd[3]);
    Button STRAT_5 (0, 270, 190, 110,strat_sd[4]);
    Button STRAT_6 (210, 270, 190, 110, strat_sd[5]);
    Button STRAT_7 (0, 390, 190, 110, strat_sd[6]);
    Button STRAT_8 (210, 390, 190, 110, strat_sd[7]);
    Button STRAT_9 (0, 510, 190, 110, strat_sd[8]);
    Button STRAT_10 (210, 510, 190, 110, strat_sd[9]);
    Button RETOUR  (0, 680, 400, 110, "--Precedent--");
    //Definition des boutons
    
    Ack_strat = 0;
    Strat = 0;
    STRAT_1.Draw(0xFFF0F0F0, 0);
    STRAT_2.Draw(0xFFF0F0F0, 0);
    STRAT_3.Draw(0xFFF0F0F0, 0);
    STRAT_4.Draw(0xFFF0F0F0, 0);
    STRAT_5.Draw(0xFFF0F0F0, 0);
    STRAT_6.Draw(0xFFF0F0F0, 0);
    STRAT_7.Draw(0xFFF0F0F0, 0);
    STRAT_8.Draw(0xFFF0F0F0, 0);
    STRAT_9.Draw(0xFFF0F0F0, 0);  
    STRAT_10.Draw(0xFFF0F0F0, 0);              
    RETOUR.Draw(0xFFFF0000, 0);
                
    while(Ack_strat == 0) 
    {
        canProcessRx();
        CANMessage msgTx=CANMessage();
        msgTx.id=ECRAN_CHOICE_STRAT;
        if (RETOUR.Touched())
            return -1;
        while(RETOUR.Touched());
                   //////////////////////////////STRATEGIE N°1
                    if (STRAT_1.Touched()){
                        Strat = 0;
                        msgTx.data[0] = 0x1;
                        can2.write(msgTx);
                        while(STRAT_1.Touched());
                        Ack_strat =1;
                        }
                    /////////////////////////////STRATEGIE N°2
                     if (STRAT_2.Touched()){
                        Strat = 1;
                        msgTx.data[0] = 0x2;
                        can2.write(msgTx);
                        while(STRAT_2.Touched());
                        Ack_strat =1;
                        }
                    //////////////////////////////STRATEGIE N°3
                     if (STRAT_3.Touched()){
                        Strat = 2;
                        msgTx.data[0] = 0x3;
                        can2.write(msgTx);
                        while(STRAT_3.Touched());
                        Ack_strat =1;
                        }
                    /////////////////////////////STRATEGIE N°4
                     if (STRAT_4.Touched()){
                        Strat = 3;
                        msgTx.data[0] = 0x4;
                        can2.write(msgTx);
                        while(STRAT_4.Touched());
                        Ack_strat =1;
                        }
                    ///////////////////////////////STRATEGIE N°5
                     if (STRAT_5.Touched()){
                        Strat = 4;
                        msgTx.data[0] = 0x5;  
                        can2.write(msgTx);                     
                        while(STRAT_5.Touched());
                        Ack_strat =1;
                        }
                    ////////////////////////////////STRATEGIE N°6
                     if (STRAT_6.Touched()){
                        Strat = 5;
                        msgTx.data[0] = 0x6;
                        can2.write(msgTx);
                        while(STRAT_6.Touched());
                        Ack_strat =1;
                        }
                    /////////////////////////////////STRATEGIE N°7
                     if (STRAT_7.Touched()){
                        Strat = 6;
                        msgTx.data[0] = 0x7;
                        can2.write(msgTx);
                        while(STRAT_7.Touched());
                        Ack_strat =1;
                        }
                    /////////////////////////////////STRATEGIE N°8
                     if (STRAT_8.Touched()){
                        Strat = 7;
                        msgTx.data[0] = 0x8;
                        can2.write(msgTx);
                        while(STRAT_8.Touched());
                        Ack_strat =1;
                        }
                    /////////////////////////////////STRATEGIE N°9
                     if (STRAT_9.Touched()){
                        Strat = 8;
                        msgTx.data[0] = 0x9;
                        can2.write(msgTx);
                        while(STRAT_9.Touched());
                        Ack_strat =1;
                    }
                    ///////////////////////////////////STRATEGIE N°10
                     if (STRAT_10.Touched()){
                        Strat = 9;
                        msgTx.data[0] = 0xA;
                        can2.write(msgTx);
                        while(STRAT_10.Touched());
                        Ack_strat =1;
                    }
     
    }
    return Strat;  
    
}

void affichage_compteur (int nombre)
{
    int dizaine=0,unite=0;
    dizaine = nombre/10;
    unite = nombre-(10*dizaine);
    print_segment(unite,0);
    print_segment(dizaine,200);
    
}


//****print_segment***
//Dessine en 7 segment le nombre en parametre
//        A 
//      =====
//     |     |
//   B |  G  | E
//     |=====|
//   C |     | F
//     |     |
//      =====
//        D
/*
position pour le chiffre des unites
lcd.FillRect(460,75,120,25);//    A
lcd.FillRect(435,100,25,120);//   B 
lcd.FillRect(435,245,25,120);//   C
lcd.FillRect(460,365,120,25);//   D
lcd.FillRect(580,100,25,120);//   E
lcd.FillRect(580,245,25,120);//   F
lcd.FillRect(460,220,120,25);//   G

position pour le chiffre des dizaines
lcd.FillRect(260,75,120,25);//    A
lcd.FillRect(235,100,25,120);//   B 
lcd.FillRect(235,245,25,120);//   C
lcd.FillRect(260,365,120,25);//   D
lcd.FillRect(380,100,25,120);//   E
lcd.FillRect(380,245,25,120);//   F
lcd.FillRect(260,220,120,25);//   G
*/
        
void print_segment(int nombre, int decalage)
{   
    switch(nombre)
    {
        case 0:
        lcd.FillRect(240-decalage,75,120,25);
        lcd.FillRect(215-decalage,100,25,120);
        lcd.FillRect(215-decalage,245,25,120);
        lcd.FillRect(360-decalage,245,25,120);
        lcd.FillRect(360-decalage,100,25,120);
        lcd.FillRect(240-decalage,365,120,25);
        break;
        
        case 1:
        lcd.FillRect(360-decalage,100,25,120);//   E
        lcd.FillRect(360-decalage,245,25,120);//   F
        break;
        
        case 2:
        lcd.FillRect(240-decalage,75,120,25);//    A 
        lcd.FillRect(215-decalage,245,25,120);//   C
        lcd.FillRect(240-decalage,365,120,25);//   D
        lcd.FillRect(360-decalage,100,25,120);//   E
        lcd.FillRect(240-decalage,220,120,25);//   G
        break;
        
        case 3:
        lcd.FillRect(240-decalage,75,120,25);//    A
        lcd.FillRect(360-decalage,100,25,120);//   E
        lcd.FillRect(240-decalage,220,120,25);//   G
        lcd.FillRect(240-decalage,365,120,25);//   D
        lcd.FillRect(360-decalage,245,25,120);//   F
        break;
        
        case 4:
        lcd.FillRect(215-decalage,100,25,120);//   B 
        lcd.FillRect(360-decalage,100,25,120);//   E
        lcd.FillRect(360-decalage,245,25,120);//   F
        lcd.FillRect(240-decalage,220,120,25);//   G
        break;
        
        case 5:
        lcd.FillRect(240-decalage,75,120,25);//    A
        lcd.FillRect(215-decalage,100,25,120);//   B
        lcd.FillRect(240-decalage,220,120,25);//   G
        lcd.FillRect(240-decalage,365,120,25);//   D
        lcd.FillRect(360-decalage,245,25,120);//   F
        break;
        
        case 6:
        lcd.FillRect(240-decalage,75,120,25);//    A
        lcd.FillRect(215-decalage,100,25,120);//   B 
        lcd.FillRect(215-decalage,245,25,120);//   C
        lcd.FillRect(240-decalage,365,120,25);//   D
        lcd.FillRect(360-decalage,245,25,120);//   F
        lcd.FillRect(240-decalage,220,120,25);//   G
        break;
        
        case 7:
        lcd.FillRect(240-decalage,75,120,25);//    A
        lcd.FillRect(360-decalage,100,25,120);//   E
        lcd.FillRect(360-decalage,245,25,120);//   F
        break;
        
        case 8:
        lcd.FillRect(240-decalage,75,120,25); //    A
        lcd.FillRect(215-decalage,100,25,120);
        lcd.FillRect(215-decalage,245,25,120);
        lcd.FillRect(360-decalage,245,25,120);//...
        lcd.FillRect(360-decalage,100,25,120);
        lcd.FillRect(240-decalage,365,120,25);
        lcd.FillRect(240-decalage,220,120,25);//   G
        break;
        
        case 9:
        lcd.FillRect(240-decalage,75,120,25);//    A
        lcd.FillRect(215-decalage,100,25,120);//   B 
        lcd.FillRect(240-decalage,365,120,25);//   D
        lcd.FillRect(360-decalage,100,25,120);//   E
        lcd.FillRect(360-decalage,245,25,120);//   F
        lcd.FillRect(240-decalage,220,120,25);//   G
        break;
    }
}

void effacer_segment(long couleur)
{
    lcd.SetTextColor(couleur);
    lcd.FillRect(240-200,75,120,25); //    A
    lcd.FillRect(215-200,100,25,120);
    lcd.FillRect(215-200,245,25,120);
    lcd.FillRect(360-200,245,25,120);//...
    lcd.FillRect(360-200,100,25,120);
    lcd.FillRect(240-200,365,120,25);
    lcd.FillRect(240-200,220,120,25);//   G
    
    lcd.FillRect(240,75,120,25); //    A
    lcd.FillRect(215,100,25,120);
    lcd.FillRect(215,245,25,120);
    lcd.FillRect(360,245,25,120);//...
    lcd.FillRect(360,100,25,120);
    lcd.FillRect(240,365,120,25);
    lcd.FillRect(240,220,120,25);//   G
}