File content as of revision 45:4f93e99bac6e:

#include "global.h"
#include <string.h>
#include <sstream>
//#include "StrategieManager.h"



#define M_PI 3.14159265358979323846
#define VERT 0xFF00FF00
#define ROUGE 0xFFFF0000
#define BLEU 0xFF0000FF
#define JAUNE 0xFFFEFE00
#define BLANC 0xFF000000
#define ORANGE 0xFFFFA500
#define NOIR 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",
};

int waitingAckID_FIN;
int waitingAckFrom_FIN;

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;
short SCORE_GLOBAL=0;
short SCORE_GR=20;
short SCORE_PR=0;

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 -> VERT | 1 -> jaune
unsigned short angleRecalage = 0;
unsigned char checkCurrent = 0;
unsigned char countAliveCard = 0;
unsigned char ligne=0;


int Fevitement=0;
int EvitEtat= 0;
int stop_evitement=0;


float angle_moyen_balise_IR = 0.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;//une fois détecté réinitialise
unsigned char ingnorBalise = 0;//0:balise ignore 1:on ecoute la balise
unsigned char robot_arrete = 0;

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

struct S_Instruction instruction;

char couleur1, couleur2, couleur3;
float cptf;
int cpt,cpt1;

typedef enum {INIT, ATT, CHOIX, DEMO, TEST_TELEMETRE, TEST_CAPTEURS, TEST_SERVO, TEST_TIR, DEMO_IMMEUBLE,DEMO_TRIEUR, 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;
E_Stratposdebut etat_pos=RECALAGE_1;

/////////////////DEFINITION DES BOUTONS////////////////////
Button COTE_VERT(0, 25, 400, 300, "JAUNE");
Button COTE_ORANGE(0, 350, 400, 300, "VIOLET");
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_HERKULEX(0, 25, 400, 100, "Test servos");
Button TEST_LASER(0, 135, 400, 100, "Test telemetre");
Button TEST_COULEURS(0,245,400,100,"Test capteurs");
Button TEST_TIR_BALLE(0,355,400,100,"Test Lanceur");
Button TEST_IMMEUBLE(0,465,400,100,"Test immeuble");
Button TEST_TRIEUR(0,575,400,100,"Test aiguilleur");
Button TIR_CHATEAU(0, 25, 400, 100, "Tir chateau");
Button EPURATION(0, 150, 400, 100, "epuration");
Button LANCEUR_ON(0,275,400,100,"allumer le lanceur");
Button LANCEUR_OFF(0,400,400,100,"eteindre le lanceur");
Button ABAISSE_BLOC(0, 25, 400, 100, "Ramasser blocs");
Button RELEVE_BLOC(0, 135, 400, 100, "lacher blocs");
Button BRAS_ABEILLE_ON(0,245,400,100,"bras abeille");
Button BRAS_ABEILLE_OFF(0,355,400,100,"baisser bras abeille");
Button INTERRUPTEUR_ON(0,465,400,100,"baisser bras interrupt");
Button INTERRUPTEUR_OFF(0,575,400,100,"baisser bras interrupt");
Button FORCE_LAUNCH(0, 50, 400, 320, "Force Launch");
Button TRI(0, 25, 400, 100, "Test tri");
Button AIGUILLEUR_D(0, 150, 400, 100, "aiguilleur droite");
Button AIGUILLEUR_G(0,275,400,100,"aiguilleur gauche");
Button AIGUILLEUR_CTRE(0,400,400,100,"aiguilleur centre");
Button SUIVANT(0,380,200,100,"Suivant");
Button COLOR_ORANGE (0, 230, 190, 110,"");
Button COLOR_JAUNE (210, 230, 190, 110,"");
Button COLOR_VERT (0, 350, 190, 110,"");
Button COLOR_BLEU (210, 350, 190, 110,"");
Button COLOR_NOIR (105, 470, 190, 110,"");
////////////////////////////////////////////////////////////

void SendRawId (unsigned short id);
void SelectionStrat (unsigned char numeroStrat);
void Setflag(void);
void can2Rx_ISR(void);
signed char Bouton_Strat (void);
signed char blocage_balise;
void print_segment(int nombre, int decalage);
void affichage_compteur (int nombre);
void effacer_segment(long couleur);

unsigned short telemetreDistance=0;
unsigned short telemetreDistance_avant_gauche=0;
unsigned short telemetreDistance_avant_droite=0;
unsigned short telemetreDistance_arriere_gauche=0;
unsigned short telemetreDistance_arriere_droite=0;

#ifdef ROBOT_BIG


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

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


unsigned short id_check[NOMBRE_CARTES]= {CHECK_MOTEUR,CHECK_BALISE};
unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_MOTEUR,ALIVE_BALISE};
InterruptIn jack(PG_11); //  entrée analogique en interruption pour le jack


#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

    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;
        blocage_balise=1;
    }
}

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


//Affiche une variable sur l'écran tactile//
void affichage_var(double Var)
{
    if(ligne==7)
        ligne=0;
    char aff[10]="toto";
    sprintf(aff,"%lf ",Var);
    lcd.DisplayStringAt(120, LINE(20+(ligne)), (uint8_t *)aff, LEFT_MODE);
    //ligne++;

}


/****************************************************************************************/
/* FUNCTION NAME: affichage_debug                                                       */
/* DESCRIPTION  : Affiche l'état de gameEtat sur l'écran lcd                            */
/****************************************************************************************/
void affichage_debug(int 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(VERT);
        lcd.DisplayStringAt(0, LINE(20+i), (uint8_t *)tableau_aff[i], LEFT_MODE);
    }
    /*while(!ack_bluetooth){    // mode pas à pas en bluetooth ou via écran
        //liaison_bluetooth();
    }
    ack_bluetooth=0;*/
    /*while(SUIVANT.Touched()==0);
    while(SUIVANT.Touched());*/
}

/****************************************************************************************/
/* FUNCTION NAME: automate_etat_ihm                                                     */
/* DESCRIPTION  : Automate de gestion de l'affichage                                    */
/****************************************************************************************/
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 : //intialise l'écran et passe à l'attente d'initialisation des cartes
            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);

            etat=ATT;
            break;

        case ATT :  //Si les cartes sont présentes passe directement à choix sinon attente de force Launch (cette partie est encore buggée mais les cartes affichent bien leur présence donc faut juste force launch tout le temps...)
            if (flag==1) {
                etat = CHOIX;
                gameEtat = ETAT_CONFIG;
            } else if (FORCE_LAUNCH.Touched()) {
                etat = CHOIX;
                gameEtat = ETAT_CONFIG;
                while(FORCE_LAUNCH.Touched());
            }

            break;


        case CHOIX :    //Match ou DEMO
            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);
            RETOUR.Draw(0xFFFF0000, 0);
            TEST_HERKULEX.Draw(VERT, 0);
            TEST_LASER.Draw(VERT, 0);
            TEST_COULEURS.Draw(VERT, 0);
            TEST_TIR_BALLE.Draw(VERT, 0);
            TEST_IMMEUBLE.Draw(VERT,0);
            TEST_TRIEUR.Draw(VERT,0);
            if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config//
                InversStrat = 0;//Pas d'inversion de la couleur
            }
            while (etat == DEMO) {      ////////////////////////////LISTE DES DIFFERENTES DEMOS POSSIBLES///////////////////////////////////////////
                canProcessRx();
                if(TEST_HERKULEX.Touched()) {
                    //Strat = 0x10;
                    while(TEST_HERKULEX.Touched());
                    CANMessage trame_Tx = CANMessage();
                    trame_Tx.len = 1;
                    trame_Tx.format = CANStandard;
                    trame_Tx.type = CANData;
                    trame_Tx.id=CHOICE_COLOR;
                    trame_Tx.data[0]=0x2;
                    can2.write(trame_Tx);
                    TEST_HERKULEX.Draw(0xFFF0F0F0, 0);
                    etat = TEST_SERVO;
                    lcd.Clear(LCD_COLOR_WHITE);
                    ModeDemo=1;
                } else if(TEST_LASER.Touched()) {
                    //Strat = 0x11;
                    while(TEST_LASER.Touched());
                    TEST_LASER.Draw(0xFFF0F0F0, 0);
                    etat = TEST_TELEMETRE;
                } else if (TEST_COULEURS.Touched()) {
                    while(TEST_COULEURS.Touched());
                    TEST_LASER.Draw(0xFFF0F0F0, 0);
                    etat =TEST_CAPTEURS ;
                }

                else if (TEST_TIR_BALLE.Touched()) {
                    while(TEST_TIR_BALLE.Touched());
                    TEST_TIR_BALLE.Draw(0xFFF0F0F0, 0);
                    etat =TEST_TIR ;
                    lcd.Clear(LCD_COLOR_WHITE);
                    CANMessage trame_Tx = CANMessage();
                    trame_Tx.len = 1;
                    trame_Tx.format = CANStandard;
                    trame_Tx.type = CANData;
                    trame_Tx.id=CHOICE_COLOR;
                    trame_Tx.data[0]=0x2;
                    can2.write(trame_Tx);
                    ModeDemo=1;
                } else if(TEST_IMMEUBLE.Touched()) {
                    while(TEST_IMMEUBLE.Touched());
                    TEST_IMMEUBLE.Draw(0xFFF0F0F0, 0);
                    etat =DEMO_IMMEUBLE;
                    lcd.Clear(LCD_COLOR_WHITE);
                } else if(TEST_TRIEUR.Touched()) {
                    while(TEST_TRIEUR.Touched());
                    etat=DEMO_TRIEUR;
                    lcd.Clear(LCD_COLOR_WHITE);
                }
                if(RETOUR.Touched()) {
                    etat = CHOIX;
                    while(RETOUR.Touched());

                }
                if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config
                    Ack_strat = 1;
                    wait_ms(10);
                }
            }
            break;
        ///////////////////////////////TESTE LES SERVOS LIES AU TRI DES BALLES///////////////////////////////
        case DEMO_TRIEUR:
            lcd.SetBackColor(LCD_COLOR_WHITE);
            lcd.DisplayStringAt(20, LINE(2), (uint8_t *)"DEMONSTRATION COURS", LEFT_MODE);
            TRI.Draw(VERT, 0);
            AIGUILLEUR_D.Draw(VERT, 0);
            AIGUILLEUR_G.Draw(VERT, 0);
            AIGUILLEUR_CTRE.Draw(VERT, 0);
            while(etat==DEMO_TRIEUR) {
                if(RETOUR.Touched()) {
                    while (RETOUR.Touched());
                    etat=DEMO;
                } else if(TRI.Touched()) {
                    while (TRI.Touched());
                    SendRawId(AIGUILLEUR_CENTRE);
                    wait(0.5);
                    SendRawId(AIGUILLEUR_DROITE);
                    wait(0.5);
                    SendRawId(AIGUILLEUR_GAUCHE);
                    wait(0.5);
                    SendRawId(AIGUILLEUR_CENTRE);

                    break;
                } else if(AIGUILLEUR_D.Touched()) {
                    while (AIGUILLEUR_D.Touched());
                    SendRawId(AIGUILLEUR_DROITE);
                    break;
                } else if(AIGUILLEUR_G.Touched()) {
                    while (AIGUILLEUR_G.Touched());
                    SendRawId(AIGUILLEUR_GAUCHE);
                    break;

                } else if(BRAS_ABEILLE_OFF.Touched()) {
                    while (BRAS_ABEILLE_OFF.Touched());
                    SendRawId(BRAS_ABEILLE_DOWN);
                    break;
                } else if(AIGUILLEUR_CTRE.Touched()) {
                    while (AIGUILLEUR_CTRE.Touched());
                    SendRawId(AIGUILLEUR_CENTRE);
                    break;
                }

            }
            break;
        case DEMO_IMMEUBLE: //TESTE LE MONTE IMMEUBLE SUIVANT UN CODE COULEUR CHOISI
            int color=0;
            lcd.SetBackColor(LCD_COLOR_WHITE);
            lcd.DisplayStringAt(20, LINE(2), (uint8_t *)"Choix du code couleur", LEFT_MODE);

            CANMessage msgTx=CANMessage();
            msgTx.id=MONTER_IMMEUBLE; // Monter immeuble
            msgTx.len=3;
            msgTx.format=CANStandard;
            msgTx.type=CANData;


            while(etat==DEMO_IMMEUBLE) {
                switch(color) {
                    case 0:

                        RETOUR.Draw(ROUGE,0);
                        COLOR_NOIR.Draw(NOIR,1);
                        COLOR_ORANGE.Draw(ORANGE,0);
                        COLOR_JAUNE.Draw(JAUNE,0);
                        COLOR_VERT.Draw(VERT,0);
                        COLOR_BLEU.Draw(BLEU,0);

                        lcd.SetBackColor(LCD_COLOR_WHITE);
                        lcd.SetTextColor(NOIR);
                        lcd.DisplayStringAt(100, LINE(4), (uint8_t *)"COULEUR 1", LEFT_MODE);
                        while(color==0) {
                            if(COLOR_ORANGE.Touched()) {
                                while(COLOR_ORANGE.Touched());
                                COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
                                msgTx.data[color]=1;
                                color++;
                            } else if (COLOR_NOIR.Touched()) {
                                while(COLOR_NOIR.Touched());
                                COLOR_NOIR.Draw(LCD_COLOR_WHITE);
                                msgTx.data[color]=2;
                                color++;
                            } else if (COLOR_VERT.Touched()) {
                                while(COLOR_VERT.Touched());
                                COLOR_VERT.Draw(LCD_COLOR_WHITE);

                                msgTx.data[color]=3;
                                color++;
                            } else if (COLOR_JAUNE.Touched()) {
                                while(COLOR_JAUNE.Touched());
                                COLOR_JAUNE.Draw(LCD_COLOR_WHITE);
                                msgTx.data[color]=4;
                                color++;
                            } else if (COLOR_BLEU.Touched()) {
                                while(COLOR_BLEU.Touched());
                                COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
                                msgTx.data[color]=5;
                                color++;
                            }
                        }
                        break;

                    case 1:
                        lcd.SetBackColor(LCD_COLOR_WHITE);
                        lcd.SetTextColor(NOIR);
                        lcd.DisplayStringAt(100, LINE(4), (uint8_t *)"COULEUR 2", LEFT_MODE);
                        if(COLOR_ORANGE.Touched()) {
                            while(COLOR_ORANGE.Touched());
                            COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
                            msgTx.data[color]=1;
                            color++;
                        } else if (COLOR_NOIR.Touched()) {
                            while(COLOR_NOIR.Touched());
                            COLOR_NOIR.Draw(LCD_COLOR_WHITE);
                            msgTx.data[color]=2;
                            color++;
                        } else if (COLOR_VERT.Touched()) {
                            while(COLOR_VERT.Touched());
                            COLOR_VERT.Draw(LCD_COLOR_WHITE);

                            msgTx.data[color]=3;
                            color++;
                        } else if (COLOR_JAUNE.Touched()) {
                            while(COLOR_JAUNE.Touched());
                            COLOR_JAUNE.Draw(LCD_COLOR_WHITE);
                            msgTx.data[color]=4;
                            color++;
                        } else if (COLOR_BLEU.Touched()) {
                            while(COLOR_BLEU.Touched());
                            COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
                            msgTx.data[color]=5;
                            color++;
                        }
                        break;

                    case 2:
                        lcd.SetBackColor(LCD_COLOR_WHITE);
                        lcd.SetTextColor(NOIR);
                        lcd.DisplayStringAt(100, LINE(4), (uint8_t *)"COULEUR 3", LEFT_MODE);
                        if(COLOR_ORANGE.Touched()) {
                            while(COLOR_ORANGE.Touched());
                            COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
                            msgTx.data[color]=1;
                            color++;
                        } else if (COLOR_NOIR.Touched()) {
                            while(COLOR_NOIR.Touched());
                            COLOR_NOIR.Draw(LCD_COLOR_WHITE);
                            msgTx.data[color]=2;
                            color++;
                        } else if (COLOR_VERT.Touched()) {
                            while(COLOR_VERT.Touched());
                            COLOR_VERT.Draw(LCD_COLOR_WHITE);

                            msgTx.data[color]=3;
                            color++;
                        } else if (COLOR_JAUNE.Touched()) {
                            while(COLOR_JAUNE.Touched());
                            COLOR_JAUNE.Draw(LCD_COLOR_WHITE);
                            msgTx.data[color]=4;
                            color++;
                        } else if (COLOR_BLEU.Touched()) {
                            while(COLOR_BLEU.Touched());
                            COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
                            msgTx.data[color]=5;
                            color++;
                        }
                        break;
                    case 3:
                        lcd.Clear(LCD_COLOR_WHITE);
                        lcd.SetBackColor(LCD_COLOR_WHITE);
                        lcd.SetTextColor(NOIR);

                        lcd.DisplayStringAt(0, LINE(4), (uint8_t *)"Immeuble en construction", LEFT_MODE);
                        RETOUR.Draw(ROUGE,0);
                        can2.write(msgTx);
                        color++;
                        break;

                    case 4:
                        if(RETOUR.Touched()) {
                            while(RETOUR.Touched());
                            etat=DEMO;
                        }
                        break;
                }
                if(RETOUR.Touched()) {
                    while(RETOUR.Touched());
                    etat=DEMO;
                }
            }
            break;





        case TEST_SERVO:        //TEST DU RESTE DES SERVOS DISPO HORS TIR
            lcd.SetBackColor(LCD_COLOR_WHITE);
            lcd.DisplayStringAt(20, LINE(2), (uint8_t *)"DEMONSTRATION COURS", LEFT_MODE);
            ABAISSE_BLOC.Draw(VERT, 0);
            RELEVE_BLOC.Draw(VERT, 0);
            BRAS_ABEILLE_ON.Draw(VERT, 0);
            BRAS_ABEILLE_OFF.Draw(VERT, 0);
            INTERRUPTEUR_ON.Draw(VERT, 0);
            INTERRUPTEUR_OFF.Draw(VERT, 0);
            RETOUR.Draw(0xFFFF0000,0);
            while(etat==TEST_SERVO) {
                if(RETOUR.Touched()) {
                    while (RETOUR.Touched());
                    etat=DEMO;
                } else if(ABAISSE_BLOC.Touched()) {
                    while (ABAISSE_BLOC.Touched());
                    SendRawId(GABARIT_PETIT_ROBOT);
                    break;
                } else if(RELEVE_BLOC.Touched()) {
                    while (RELEVE_BLOC.Touched());
                    SendRawId(PRESENTOIR_AVANT);
                    break;
                } else if(BRAS_ABEILLE_ON.Touched()) {
                    while (BRAS_ABEILLE_ON.Touched());
                    SendRawId(BRAS_ABEILLE_UP);
                    break;

                } else if(BRAS_ABEILLE_OFF.Touched()) {
                    while (BRAS_ABEILLE_OFF.Touched());
                    SendRawId(BRAS_ABEILLE_DOWN);
                    break;
                } else if(INTERRUPTEUR_ON.Touched()) {
                    while (INTERRUPTEUR_ON.Touched());
                    SendRawId(ALLUMER_PANNEAU_UP);
                    break;
                } else if(INTERRUPTEUR_OFF.Touched()) {
                    while (INTERRUPTEUR_OFF.Touched());
                    SendRawId(ALLUMER_PANNEAU_DOWN);
                    break;
                }
            }
            break;

        case TEST_TIR:  // TEST DES FONCTIONS LIEES AUX TIRS
            lcd.SetBackColor(LCD_COLOR_WHITE);
            lcd.DisplayStringAt(20, LINE(2), (uint8_t *)"DEMONSTRATION COURS", LEFT_MODE);
            TIR_CHATEAU.Draw(VERT, 0);
            EPURATION.Draw(VERT, 0);
            LANCEUR_ON.Draw(VERT, 0);
            LANCEUR_OFF.Draw(VERT, 0);
            RETOUR.Draw(ROUGE, 0);
            while(etat==TEST_TIR) {
                if(TIR_CHATEAU.Touched()) {
                    while (TIR_CHATEAU.Touched());
                    SendRawId(INCLINAISON_CHATEAU);
                    break;
                } else if (EPURATION.Touched()) {
                    while (EPURATION.Touched());
                    SendRawId(INCLINAISON_EPURATION);
                    break;
                } else if(LANCEUR_ON.Touched()) {
                    while (LANCEUR_ON.Touched());
                    CANMessage msgTx=CANMessage();
                    msgTx.format=CANStandard;
                    msgTx.type=CANData;
                    msgTx.id=LANCEMENT_MOTEUR_TIR_ON;

                    msgTx.len=1;
                    msgTx.data[0]=0;
                    can2.write(msgTx);
                    break;
                } else if(LANCEUR_OFF.Touched()) {
                    while (LANCEUR_OFF.Touched());
                    SendRawId(LANCEMENT_MOTEUR_TIR_OFF);
                    break;
                } else if (RETOUR.Touched()) {
                    while (RETOUR.Touched());
                    etat=DEMO;

                }
            }
            break;



        case TEST_TELEMETRE:    //AFFICHAGE DE LA VALEUR LUE PAR LES 4 TELEMETRES
            ModeDemo=1;
            lcd.Clear(LCD_COLOR_WHITE);
            lcd.SetBackColor(LCD_COLOR_WHITE);
            lcd.DisplayStringAt(20, LINE(2), (uint8_t *)"DEMONSTRATION COURS", LEFT_MODE);
            RETOUR.Draw(0xFFFF0000, 0);
            while(etat==TEST_TELEMETRE) {
                SendRawId(DATA_RECALAGE);
                wait(0.1);
                canProcessRx();
                if(RETOUR.Touched()) {
                    while( RETOUR.Touched());
                    etat=DEMO;
                    lcd.Clear(LCD_COLOR_WHITE);
                }
            }
            break;      ///////////////////////////////////////////FIN DES DEMOS/////////////////////////////////////////////////


        case SELECT_SIDE :      // CHOIX DU COTE DU TERRAIN + INVERSION DE LA STRAT SI COTE ORANGE+ ENVOI DU COTE A LA CARTE CAPTEUR/ACTIONNEURS
            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_VERT.Draw(LCD_COLOR_YELLOW, 0);
            COTE_ORANGE.Draw(LCD_COLOR_DARKMAGENTA, 0);
            RETOUR.Draw(LCD_COLOR_RED, 0);


            while (etat == SELECT_SIDE) {
                canProcessRx();
                if(COTE_VERT.Touched()) {
                    Cote = 0x0;
                    InversStrat = Cote;
                    etat = TACTIQUE;
                    CANMessage trame_Tx = CANMessage();
                    trame_Tx.len = 1;
                    trame_Tx.format = CANStandard;
                    trame_Tx.type = CANData;
                    trame_Tx.id=CHOICE_COLOR;
                    trame_Tx.data[0]=Cote;
                    can2.write(trame_Tx);
                    while(COTE_VERT.Touched());

                }

                if(COTE_ORANGE.Touched()) {
                    Cote = 0x1;
                    InversStrat= Cote;
                    etat = TACTIQUE;
                    CANMessage trame_Tx = CANMessage();
                    trame_Tx.len = 1;
                    trame_Tx.format = CANStandard;
                    trame_Tx.type = CANData;
                    trame_Tx.id=CHOICE_COLOR;
                    trame_Tx.data[0]=Cote;
                    can2.write(trame_Tx);
                    while(COTE_ORANGE.Touched());
                }

                if(RETOUR.Touched()) {
                    etat = CHOIX;
                    while(RETOUR.Touched());
                }
            }

            break;

        case TACTIQUE : //AFFICHE LA LISTE DES STRATS AFIN DE SELECTIONNER CELLE VOULUE
            if (Cote == 0) {
                lcd.Clear(VERT);
                lcd.SetBackColor(VERT);
            } else if (Cote == 1) {
                lcd.Clear(ORANGE);
                lcd.SetBackColor(ORANGE);
            } 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 :  //SECONDE VALIDATION DE LA STRAT
            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); //affiche la stratégie selectionnée

            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 : //FONCTIONS D'AFFICHAGE DE L'ATTENTE DU JACK
            lcd.Clear(BLANC);
            lcd.SetBackColor(LCD_COLOR_WHITE);
            lcd.SetTextColor(LCD_COLOR_BLACK);

            if (Cote == 0) {
                lcd.Clear(VERT);
                lcd.SetBackColor(VERT);
            } else if (Cote == 1) {
                lcd.Clear(ORANGE);
                lcd.SetBackColor(ORANGE);
            } else {
                lcd.Clear(VERT);
                lcd.SetBackColor(VERT);
            }
            canProcessRx();
            lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"En attente du Jack", CENTER_MODE);
            etat=WAIT_JACK;
            break;

        case WAIT_JACK: //VERITABLE ATTENTE DU JACK
            break;

        case COMPTEUR:  //PEUT AFFICHER UN COMPTEUR DU TEMPS RESTANT AVANT LA FIN DE LA PARTIE OU BIEN TRES UTILE POUR PRINT DES VARIABLES CHAQUE SEC EX: gameEtat
            cptf=gameTimer.read();
            lcd.SetTextColor(LCD_COLOR_BLACK);
            cpt=(int)cptf;
            if(cpt != cpt1) {
                lcd.Clear(VERT);
                // affichage_compteur(100-cpt);
                //affichage_compteur(SCORE_PR);
#ifdef ROBOT_BIG
                affichage_var(SCORE_GR);
#else
                affichage_var(SCORE_PR);
#endif
                if(liaison_pr.paquet_en_attente()) {
                    PaquetDomotique *paquet=liaison_pr.lire();
                    if(paquet->identifiant==PAQUET_IDENTIFIANT_AJOUTERSCORE) {
                        SCORE_PR+=convertir_score(paquet);
                    }
                    delete paquet;
                }
            }
            cpt1=cpt;
            flag_timer=0;

            //affichage_debug(gameEtat);
            lcd.SetBackColor(LCD_COLOR_WHITE);

            break;

        case FIN :  //AFFICHAGE DE FIN AVEC LE SCORE FINAL
            lcd.Clear (LCD_COLOR_WHITE);
            lcd.SetBackColor(LCD_COLOR_WHITE);
#ifdef ROBOT_BIG
            // affichage_compteur(SCORE_GR);
            affichage_var(SCORE_GR);
            //liaison_Tx.envoyer_short(PAQUET_IDENTIFIANT_FINMATCH,SCORE_GLOBAL);
#else
            //affichage_compteur(SCORE_PR);
            affichage_var(SCORE_PR);
#endif
            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() >= 99000) {//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;
                        SendRawId(ECRAN_ALL_CHECK);
                        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;

            SendRawId(GLOBAL_START);

            gameEtat = ETAT_GAME_WAIT_FOR_JACK;
            if (etat == TEST_TELEMETRE|| etat ==TEST_CAPTEURS || etat == TEST_SERVO || etat ==TEST_TIR || etat == DEMO_IMMEUBLE) {
                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

            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,1800,localData2);

            instruction = strat_instructions[actual_instruction];
            //On effectue le traitement de l'instruction

            break;
        case ETAT_GAME_WAIT_FOR_JACK:
            if(instruction.order==POSITION_DEBUT) {
                switch(etat_pos) {      // AUTOMATE PERMETTANT AU ROBOT DE SE POSITIONNER TOUT SEUL AU DEBUT DE LA PARTIE (Ne PAS RETIRER LE JACK PENDANT CE TEMPS !!!)
                    case RECALAGE_1:
                        waitingAckID = ASSERVISSEMENT_RECALAGE;
                        waitingAckFrom = ACKNOWLEDGE_MOTEUR;
#ifdef ROBOT_SMALL
                        GoStraight(3000, 1,MOITIEE_ROBOT-5, 0);  //on se recale contre le mur donc il faut donner la valeur du centre du robot (les -5 qui trainent sont dus au tables pourraves sur place)
#else
                        GoStraight(-3000, 1,MOITIEE_ROBOT-5, 0);
#endif
                        while(waitingAckID !=0 && waitingAckFrom !=0)
                            canProcessRx();
                        waitingAckID_FIN=ASSERVISSEMENT_RECALAGE;
                        waitingAckFrom_FIN= INSTRUCTION_END_MOTEUR;
                        while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
                            canProcessRx();
                        etat_pos=RECULER_1;
                        break;

                    case RECULER_1:
                        waitingAckID = ASSERVISSEMENT_RECALAGE;
                        waitingAckFrom = ACKNOWLEDGE_MOTEUR;
#ifdef ROBOT_SMALL
                        GoStraight(-450, 0, 0, 0);//-450
#else
                        GoStraight(150, 0, 0, 0);
#endif
                        while(waitingAckID !=0 && waitingAckFrom !=0)
                            canProcessRx();
                        waitingAckID_FIN=ASSERVISSEMENT_RECALAGE;
                        waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
                        while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
                            canProcessRx();
                        etat_pos=TOURNER;
                        break;

                    case TOURNER:
                        waitingAckID = ASSERVISSEMENT_ROTATION;
                        waitingAckFrom = ACKNOWLEDGE_MOTEUR;
                        if(Cote==0) {
                            localData2 = 900;
                        } else {
                            localData2=-900;
                        }
                        Rotate(localData2);
                        while(waitingAckID !=0 && waitingAckFrom !=0)
                            canProcessRx();
                        waitingAckID_FIN=ASSERVISSEMENT_ROTATION;
                        waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
                        while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
                            canProcessRx();
                        etat_pos=RECALAGE_2;
                        break;

                    case RECALAGE_2:
                        waitingAckID = ASSERVISSEMENT_RECALAGE;
                        waitingAckFrom = ACKNOWLEDGE_MOTEUR;
                        if(Cote==1)
                            localData3=3000-(MOITIEE_ROBOT-5);
                        else
                            localData3=MOITIEE_ROBOT;
#ifdef ROBOT_SMALL
                        GoStraight(3000, 2,localData3, 0);  //on se recale contre le mur donc il faut donner la valeur du centre du robot
#else
                        GoStraight(-3000, 2,localData3, 0);
#endif
                        while(waitingAckID !=0 && waitingAckFrom !=0)
                            canProcessRx();
                        waitingAckID_FIN=ASSERVISSEMENT_RECALAGE;
                        waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
                        while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
                            canProcessRx();
                        etat_pos=RECULER_2;
                        break;

                    case RECULER_2:
                        waitingAckID = ASSERVISSEMENT_RECALAGE;
                        waitingAckFrom = ACKNOWLEDGE_MOTEUR;
#ifdef ROBOT_SMALL
                        GoStraight(-450, 0, 0, 0);
#else
                        GoStraight(200, 0, 0, 0);
#endif
                        while(waitingAckID !=0 && waitingAckFrom !=0)
                            canProcessRx();
                        waitingAckID_FIN=ASSERVISSEMENT_RECALAGE;
                        waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
                        while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
                            canProcessRx();
                        etat_pos=GOTOPOS;
                        break;

                    case GOTOPOS:
                        localData1 = -1;

                        if(InversStrat == 1 && ingnorInversionOnce == 0) {
                            localData2 = -instruction.arg3;
                            localData3 = 3000 - instruction.arg2;//Inversion du Y
                        } else {
                            localData3 = instruction.arg2;
                            localData2 = instruction.arg3;
                        }

                        GoToPosition(instruction.arg1,localData3,localData2,localData1);
                        waitingAckID = ASSERVISSEMENT_XYT;
                        waitingAckFrom = ACKNOWLEDGE_MOTEUR;

                        while(waitingAckID !=0 && waitingAckFrom !=0)
                            canProcessRx();
                        waitingAckID_FIN=ASSERVISSEMENT_XYT;
                        waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
                        while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
                            canProcessRx();
                        etat_pos=FIN_POS;
                        break;
                    case FIN_POS:
                        actual_instruction = instruction.nextLineOK;
                        break;
                }
            }


            break;
        case ETAT_GAME_START:

            gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;

            if (ModeDemo == 0) {
                chronoEnd.attach(&chronometre_ISR,100);//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);
            //affichage_debug(gameEtat);
            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
                    }
                    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;
                        }
                    }
                    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;
                        }

                    }
                    if(InversStrat == 1 && ingnorInversionOnce == 0) {
                        localData2 = -localData2;
                    }
                    waitingAckID = ASSERVISSEMENT_ROTATION;
                    waitingAckFrom = ACKNOWLEDGE_MOTEUR;
                    Rotate(localData2);


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

                    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:
                    if(instruction.nextActionType == MECANIQUE) {
                        instruction.nextActionType = WAIT;

                        waitingAckID = ASSERVISSEMENT_RECALAGE;
                        waitingAckFrom = ACKNOWLEDGE_MOTEUR;

                        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);
                    } else { //CAPTEUR
                        SendRawId(DATA_RECALAGE);
                        waitingAckID = RECEPTION_RECALAGE;
                        waitingAckFrom = ACKNOWLEDGE_TELEMETRE;

                        // On attend que les variables soient actualisé
                        while(!(waitingAckID == 0 && waitingAckFrom == 0))
                            canProcessRx();
                        while(!(waitingAckID_FIN==0 && waitingAckFrom_FIN==0))
                            canProcessRx();

                        if(instruction.precision == RECALAGE_Y) {  // ((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))  (theta_robot < 900 && theta_robot > -900)
                            SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, recalageDistanceY(), theta_robot);
                        } else if(instruction.precision == RECALAGE_X) {
                            SetOdometrie(ODOMETRIE_SMALL_POSITION, recalageDistanceX(), y_robot, theta_robot);
                        } else if(instruction.precision == RECALAGE_T) {
                            SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, y_robot, recalageAngulaireCapteur() );
                        }
                    }
                    break;

                case ACTION:
                    int tempo = 0;
                    waitingAckID= ACK_ACTION;       //On veut un ack de type action
                    waitingAckFrom = ACKNOWLEDGE_HERKULEX; //de la part des herkulex
                    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:
            canProcessRx();

            if(waitingAckID == 0 && waitingAckFrom == 0) {//Les ack ont été reset, c'est bon on continue
                //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) { ///Actualisation des waiting ack afin d'attendre la fin des actions
                    /*wait_ms(200);
                    #ifdef ROBOT_BIG
                        SetOdometrie(ODOMETRIE_BIG_POSITION, x_robot, y_robot, theta_robot);
                    #else
                        SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, y_robot, theta_robot);
                    #endif
                    wait_ms(200);*/

                    gameEtat = ETAT_GAME_WAIT_END_INSTRUCTION;
                    switch(instruction.order) {
                        case MV_COURBURE:
                            waitingAckID_FIN = ASSERVISSEMENT_COURBURE;
                            waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
                            break;
                        case MV_LINE:
                            waitingAckID_FIN = ASSERVISSEMENT_RECALAGE;
                            waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
                            break;
                        case MV_TURN:
                            waitingAckID_FIN = ASSERVISSEMENT_ROTATION;
                            waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
                            break;
                        case MV_XYT:
                            waitingAckID_FIN = ASSERVISSEMENT_XYT;
                            waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
                            break;
                        case MV_RECALAGE:
                            waitingAckID_FIN = ASSERVISSEMENT_RECALAGE;
                            waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
                            break;
                        case ACTION:

                            if (modeTelemetre == 0) {
                                if (telemetreDistance == 0) {
                                    waitingAckID_FIN = ACK_FIN_ACTION;// ack de type action
                                    waitingAckFrom_FIN = ACKNOWLEDGE_HERKULEX; //de la part des herkulex/actionneurs
                                } else if(telemetreDistance == 5000) {
                                    // on est dans le cas ou l'on fait une ligne suivant la distance du telemetre
                                    waitingAckID_FIN = ASSERVISSEMENT_RECALAGE;
                                    waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
                                    telemetreDistance = 0;
                                }
                            } else { // si on attend la reponse du telemetre
                                //modeTelemetre = 1;
                                waitingAckID_FIN = OBJET_SUR_TABLE;
                                waitingAckFrom_FIN = 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 () > 1000) {
                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:
            canProcessRx();
            if(waitingAckID_FIN == 0 && waitingAckFrom_FIN ==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_EVITEMENT;
                if(Fevitement==1) {
                    EvitEtat= 0;
                    Fevitement=0;
                }

                /*-------------------------------------
                code origine

                if(instruction.nextLineOK != instruction.nextLineError)
                {
                    actual_instruction = instruction.nextLineError;
                    gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
                }----------------------------------------*/

            }
            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


            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;
                    ///////cv
                    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_EVITEMENT :

            /*disposition des waypoint sur le terrain
            on considère le terrain comme un rectangle de 3m par 1m50 , on exclus la zone de la rampe
             ______________
            |              |   format(x,y)en mm
            Y|    x    x    |   (1000, 2250)  (1500 , 2250)
            |              |
            |    x    x    |   (1000 , 1500) (1500 , 1500)
            |              |
            |    x    x    |   (1000 , 750)  (1500 , 750)
            |              |
            |______________|
                          X-->
            */
            static short x_terrain=3000;
            static short y_terrain=1500;

            static float x_cote_droit[3]= {0};
            static float y_cote_droit[3]= {0};
            static float x_cote_gauche[3]= {0};
            static float y_cote_gauche[3]= {0};
            static short cote=0;
//---------------------------
            /* short x_robot=500;
             short y_robot=500;
             short theta_robot=0;
            //---------------------------
             short target_x_robot=2000;
             short target_y_robot=1000;
             short target_theta_robot=1000;*/
//--------------------------
            float dist_robot_adversaire=650;//distance à laquelle on s'arrete grace à la balise
            int proxy=400;//distance entre point de controle et obstacle/adversaire
            int proximity=400;//distance entre l'objectif et obstacle/adversaire
            short taille_petit=150;// distance proxymité max mur
//---------------------------*
            static unsigned short distance=50000;//valeur impossible
            static unsigned short distance_prev=50000;
            static unsigned short theta1,theta2,theta_adversaire;

            switch(EvitEtat) {
                case 0:

                    /*SendRawId(DATA_RECALAGE);
                    wait(0.1);
                    canProcessRx();
                    unsigned short distance_adverse=telemetreDistance_arriere_gauche;   //on sauvegarde notre distance au robot
                     */
                    ingnorBalise=1;
                    Rotate(300); //on tourne a gauche pour scanner
                    waitingAckID = ASSERVISSEMENT_ROTATION;
                    waitingAckFrom = ACKNOWLEDGE_MOTEUR;
                    while(waitingAckID !=0 && waitingAckFrom !=0)
                        canProcessRx();

                    waitingAckID_FIN = ASSERVISSEMENT_ROTATION;
                    waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
                    while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0) {
                        canProcessRx();
                        SendRawId(DATA_RECALAGE);
                        distance=telemetreDistance_arriere_gauche;   //on sauvegarde notre distance au robot
                        if(distance<=distance_prev) {
                            distance_prev=distance;
                            theta1=theta_robot;
                        }
                    }
                    Rotate(-600);//on tourne a droite pour scanner
                    waitingAckID = ASSERVISSEMENT_ROTATION;
                    waitingAckFrom = ACKNOWLEDGE_MOTEUR;
                    while(waitingAckID !=0 && waitingAckFrom !=0)
                        canProcessRx();
                        
                    waitingAckID_FIN = ASSERVISSEMENT_ROTATION;
                    waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
                    while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0)
                        canProcessRx();
                        
                    Rotate(300);//on revien au centre 
                    waitingAckID = ASSERVISSEMENT_ROTATION;
                    waitingAckFrom = ACKNOWLEDGE_MOTEUR;
                    while(waitingAckID !=0 && waitingAckFrom !=0)
                        canProcessRx();

                    waitingAckID_FIN = ASSERVISSEMENT_ROTATION;
                    waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
                    while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0) {
                        canProcessRx();
                        SendRawId(DATA_RECALAGE);
                        distance=telemetreDistance_arriere_gauche;   //on sauvegarde notre distance au robot
                        if(distance<=distance_prev) {
                            distance_prev=distance;
                            theta2=theta_robot;
                        }
                    }
                    theta_adversaire=(theta1+theta2)/2;//version avec telemetre
                    //theta_adversaire=theta_robot;//version sans telemetre (robot en face
                    
                    short ang_target = (short)((atan2((float)(target_y_robot - y_robot), (float)(target_x_robot - x_robot)) * 1800 / M_PI) - theta_robot + 7200) % 3600;
                    // On passe le résultat entre -1800 et 1800
                    if (ang_target > 1800) ang_target = (ang_target- 3600);

                    // float dist_target = (short)sqrt((target_x_robot - x_robot)*(target_x_robot - x_robot)+(target_y_robot - y_robot)*(target_y_robot - y_robot));

                    float x_robot_adversaire = x_robot + (dist_robot_adversaire)*cos((float)(theta_adversaire)* M_PI/1800);
                    float y_robot_adversaire = y_robot + (dist_robot_adversaire)*sin((float)(theta_adversaire)*M_PI/1800);

                    x_cote_droit[0] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target+1300)*M_PI/1800);
                    y_cote_droit[0] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target+1300)*M_PI/1800);
                    x_cote_gauche[0] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target-1300)*M_PI/1800);
                    y_cote_gauche[0] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target-1300)*M_PI/1800);

                    x_cote_droit[1] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target+900)*M_PI/1800);
                    y_cote_droit[1] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target+900)*M_PI/1800);
                    x_cote_gauche[1] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target-900)*M_PI/1800);
                    y_cote_gauche[1] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target-900)*M_PI/1800);

                    x_cote_droit[2] = x_robot_adversaire  + (proxy)*cos((float)(theta_adversaire+ang_target+500)*M_PI/1800);
                    y_cote_droit[2] = y_robot_adversaire  + (proxy)*sin((float)(theta_adversaire+ang_target+500)*M_PI/1800);
                    x_cote_gauche[2] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target-500)*M_PI/1800);
                    y_cote_gauche[2] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target-500)*M_PI/1800);

                    SendRawId(0x0D0);//calcul

                    //for(int i=0; i<3; i++) printf("point:%d | gauche(%.1f , %.1f) | droite(%.1f , %.1f)\n\r",i,x_cote_gauche[i],y_cote_gauche[i],x_cote_droit[i],y_cote_droit[i]) ;
                    //printf("------------\n\r");
                    //-------------------------process------------------------------------------------

                    bool cote_droit=false, cote_gauche=false;


                    for (int i=0; i<3; i++) {
                        if (x_cote_droit[i]>taille_petit && x_cote_droit[i]<x_terrain-taille_petit && y_cote_droit[i] >taille_petit && y_cote_droit[i] < y_terrain-taille_petit) {
                            cote_droit=true;
                            cote=1;
                        } else {
                            cote_droit=false;
                            break;
                        }

                        if (x_cote_gauche[i]>taille_petit && x_cote_gauche[i]<x_terrain-taille_petit && y_cote_gauche[i] >taille_petit && y_cote_gauche[i] < y_terrain-taille_petit) {
                            cote_gauche=true;
                            cote=-1;
                        } else {
                            cote_gauche=false;
                            break;
                        }
                    }

                    if(!cote_droit && !cote_gauche)cote=0;

                    if (cote_droit && cote_gauche) {
                        if (ang_target<=0) {
                            cote = -1;// cote gauche
                            SendRawId(0x1D0);
                        } else if (ang_target>0) {
                            cote = 1; //cote droite
                            SendRawId(0x2D0);
                        }
                    }

                    if ( ang_target>600 || ang_target<-600)cote=0;


                    if (!cote_droit && !cote_gauche) {
                        cote=0;
                    }

                    //--------------------test target --------------------------------------

                    if ((x_robot_adversaire >= target_x_robot-proximity && x_robot_adversaire <= target_x_robot+proximity)&&(y_robot_adversaire >= target_y_robot-proximity && y_robot_adversaire <= target_y_robot+proximity)) cote=0;


                    EvitEtat = 1;

                    break;

                case 1 ://on attend la fin de la première rotation pour activer la balise
                    //ingnorBalise=1;
                    SendRawId(0x0D1);//init evitement
                    if(cote!=0) {
                        for(int i=0; i<3; i++) {
                            if(cote==-1) {
                                GoToPosition(y_cote_droit[i],x_cote_droit[i],theta_robot,1);//
                                SendRawId(0x1D1);//evitement a gauche
                            } else if(cote==1) {
                                GoToPosition(y_cote_gauche[i],x_cote_gauche[i],theta_robot,1);
                                SendRawId(0x2D1);//evitement a droite
                            }
                            waitingAckID = ASSERVISSEMENT_XYT;
                            waitingAckFrom = ACKNOWLEDGE_MOTEUR;
                            while(waitingAckID !=0 && waitingAckFrom !=0)
                                canProcessRx();

                            Fevitement=1;
                            ingnorBalise=1;
                            waitingAckID_FIN = ASSERVISSEMENT_XYT_ROTATE;
                            waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
                            while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
                                canProcessRx();

                            ingnorBalise=0;
                            waitingAckID_FIN = ASSERVISSEMENT_XYT_LINE;
                            waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
                            while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0)
                                canProcessRx();

                            ingnorBalise=1;
                            waitingAckID_FIN = ASSERVISSEMENT_XYT;
                            waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
                            while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0)
                                canProcessRx();

                        }
                    } else {
                        SendRawId(0x3D1);//cote=0 evitement non possible
                        EvitEtat=0;
                        gameEtat=ETAT_WARNING_END_LAST_INSTRUCTION;
                        ingnorBalise=0;
                        Fevitement=0;
                    }
                    EvitEtat=2;
                    break;

                case 2:
                    SendRawId(0x0D2);
                    GoToPosition(target_x_robot,target_y_robot,target_theta_robot,1);
                    waitingAckID = ASSERVISSEMENT_XYT;
                    waitingAckFrom = ACKNOWLEDGE_MOTEUR;
                    while(waitingAckID !=0 && waitingAckFrom !=0)
                        canProcessRx();

                    Fevitement=1;
                    waitingAckID_FIN = ASSERVISSEMENT_XYT;
                    waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
                    while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0)
                        canProcessRx();

                    EvitEtat=3;
                    break;

                case 3: //on charge l'instruction suivante et sort de l'evitement
                    actual_instruction++;//on charge l'instruction suivante
                    gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
                    EvitEtat = 0;
                    ingnorBalise=0;
                    Fevitement=0;
                    break;
            }

            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 message[10]="toto";
    char message1[10]="toto";
    char message2[10]="toto";
    char message3[10]="toto";
    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 ALIVE_MOTEUR:
                if (etat == ATT) {

                    lcd.SetTextColor(LCD_COLOR_LIGHTGREEN);
                    lcd.FillRect(0,400,400,150);
                    lcd.SetTextColor(LCD_COLOR_BLACK);
                    lcd.SetBackColor(LCD_COLOR_LIGHTGREEN);
                    lcd.DisplayStringAt(80, 450, (uint8_t *)"Carte Moteur", LEFT_MODE);
                }
                break;

            case ALIVE_BALISE:
                if (etat == ATT) {

                    lcd.SetTextColor(LCD_COLOR_LIGHTGREEN);
                    lcd.FillRect(0,600,400,150); //carte AX12
                    lcd.SetTextColor(LCD_COLOR_BLACK);
                    lcd.SetBackColor(LCD_COLOR_LIGHTGREEN);
                    lcd.DisplayStringAt(110, 650, (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:    //pas de break donc passe directement dans ECRAN_ALL_CHECK mais conserve l'ident initial
            case ALIVE_ACTIONNEURS_ARRIERE:
            case ALIVE_HERKULEX:
            case ECRAN_ALL_CHECK:
                if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id) {
                    waitingAckFrom = 0;//C'est la bonne carte qui indique qu'elle est en ligne
                }
                flag=1;
                break;




            /////////////////////////////////////Acknowledges de Reception de la demande d'action////////////////////////////////////////
            case ACKNOWLEDGE_HERKULEX:
            case ACKNOWLEDGE_BALISE:    //pas de break donc passe directement dans ACK_FIN_ACTION mais conserve l'ident initial

            case ACKNOWLEDGE_TELEMETRE:
            /////////////////////////////////////////////Acknowledges de la fin d'action/////////////////////////////////////////////////
            case ACKNOWLEDGE_MOTEUR:
            case INSTRUCTION_END_BALISE:
            case ACK_FIN_ACTION:
            case INSTRUCTION_END_MOTEUR:
                unsigned short recieveAckID;// = (unsigned short)msgRxBuffer[FIFO_lecture].data[0]  | ( ((unsigned short)msgRxBuffer[FIFO_lecture].data[1]) <<8);
                memcpy(&recieveAckID, msgRxBuffer[FIFO_lecture].data, 2);
                Send2Short(0x5D7,waitingAckFrom_FIN,  waitingAckID_FIN);
                Send2Short(0x5D8,msgRxBuffer[FIFO_lecture].id,  recieveAckID);

                if( waitingAckFrom == msgRxBuffer[FIFO_lecture].id && recieveAckID == waitingAckID ) {
                    waitingAckFrom = 0;
                    waitingAckID = 0;
                }
                if( waitingAckFrom_FIN == msgRxBuffer[FIFO_lecture].id && recieveAckID == waitingAckID_FIN ) {
                    //SendRawId(0x5D9);
                    waitingAckFrom_FIN = 0;
                    waitingAckID_FIN = 0;
                }

                /*
                               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;
                               }
                               if(waitingAckFrom_FIN == msgRxBuffer[FIFO_lecture].id && ((unsigned short)msgRxBuffer[FIFO_lecture].data[0]
                               |(((unsigned short)msgRxBuffer[FIFO_lecture].data[1])<<8) == waitingAckID_FIN))
                               {
                                   waitingAckFrom_FIN = 0;
                                   waitingAckID_FIN = 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 ACK_ACTION:
                if(waitingAckID == msgRxBuffer[FIFO_lecture].id) {
                    waitingAckFrom = 0;
                    waitingAckID = 0;
                }
                break;

            case BALISE_DANGER :
                SendAck(ACKNOWLEDGE_BALISE, BALISE_END_DANGER);
                break;

            case BALISE_STOP:
                SendAck(ACKNOWLEDGE_BALISE, BALISE_STOP);

                signed char fin_angle_detection = msgRxBuffer[FIFO_lecture].data[0] & 0x0F;
                signed char debut_angle_detection = (msgRxBuffer[FIFO_lecture].data[0] & 0xF0) >> 4;




                if(debut_angle_detection > fin_angle_detection) {
                    angle_moyen_balise_IR = (float)debut_angle_detection + ((15.0f-(float)debut_angle_detection)+(float)fin_angle_detection)/2.0f;
                    if(angle_moyen_balise_IR > 15.0f)
                        angle_moyen_balise_IR-=15.0f;
                } else
                    angle_moyen_balise_IR = debut_angle_detection + (fin_angle_detection-debut_angle_detection)/2;

#ifdef ROBOT_BIG
                float seuil_bas_avant = 12.0;  // >=
                float seuil_haut_avant = 0.0; // <=
                float seuil_bas_arriere = 4.0;
                float seuil_haut_arriere = 8.0;
#else
                float seuil_bas_avant = 13.0;
                float seuil_haut_avant = 15.0;
                float seuil_bas_arriere = 5.0;
                float seuil_haut_arriere = 7.0;
#endif



                if (instruction.order == MV_LINE &&   instruction.direction == FORWARD  && angle_moyen_balise_IR >= seuil_bas_avant && angle_moyen_balise_IR <= seuil_haut_avant
                        || instruction.order == MV_LINE && instruction.direction == BACKWARD && angle_moyen_balise_IR >= seuil_bas_arriere && angle_moyen_balise_IR <= seuil_haut_arriere
                        || instruction.order == MV_COURBURE && angle_moyen_balise_IR >= seuil_bas_avant && angle_moyen_balise_IR <= seuil_haut_avant
                        || instruction.order == MV_COURBURE && angle_moyen_balise_IR >= seuil_bas_arriere  && angle_moyen_balise_IR <= seuil_haut_arriere
                        || instruction.order == MV_XYT && angle_moyen_balise_IR >= seuil_bas_avant  && angle_moyen_balise_IR <= seuil_haut_avant
                        || instruction.order == MV_XYT && angle_moyen_balise_IR >= seuil_bas_arriere  && angle_moyen_balise_IR <= seuil_haut_arriere ) { //J'ai rajouté cette ligne mais il faut tester avec et sans pour voir le comportement du robot,


                    if(needToStop() != 0 && ingnorBaliseOnce ==0 && ingnorBalise==0) {
                        if(gameEtat > ETAT_GAME_START && gameEtat != ETAT_WARNING_TIMEOUT) {
                            SendRawId(0x0D9);//balise_stop
                            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
                            }
                            //stop_evitement=1;
                            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 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;*/

            case RECEPTION_DATA:
                telemetreDistance=char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[0], msgRxBuffer[FIFO_lecture].data[1]);
                telemetreDistance= (float)telemetreDistance*100.0f*35.5f+50.0f;
                waitingAckFrom = 0;
                waitingAckID = 0;
                break;

            case RECEPTION_RECALAGE:
                telemetreDistance_arriere_droite = char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[0], msgRxBuffer[FIFO_lecture].data[1]); //on récupère la distance traité par l'autre micro
                telemetreDistance_avant_droite   = char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[2], msgRxBuffer[FIFO_lecture].data[3]);
                telemetreDistance_arriere_gauche = char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[4], msgRxBuffer[FIFO_lecture].data[5]);
                telemetreDistance_avant_gauche   = char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[6], msgRxBuffer[FIFO_lecture].data[7]);



                if(ModeDemo==1) {
                    sprintf(message,"%04d mm",telemetreDistance_arriere_droite);
                    lcd.SetBackColor(LCD_COLOR_WHITE);
                    lcd.DisplayStringAt(0, LINE(10),(unsigned char *)"LASER ARD : ",LEFT_MODE);
                    lcd.DisplayStringAt(200, LINE(10),(unsigned char *)message, LEFT_MODE);

                    sprintf(message1,"%04d mm",telemetreDistance_avant_droite);
                    lcd.SetBackColor(LCD_COLOR_WHITE);
                    lcd.DisplayStringAt(0, LINE(12),(unsigned char *)"LASER AVD : ",LEFT_MODE);
                    lcd.DisplayStringAt(200, LINE(12),(unsigned char *)message1, LEFT_MODE);

                    sprintf(message2,"%04d mm",telemetreDistance_arriere_gauche);
                    lcd.SetBackColor(LCD_COLOR_WHITE);
                    lcd.DisplayStringAt(0, LINE(14),(unsigned char *)"LASER ARG : ",LEFT_MODE);
                    lcd.DisplayStringAt(200, LINE(14),(unsigned char *)message2, LEFT_MODE);

                    sprintf(message3,"%04d mm",telemetreDistance_avant_gauche);
                    lcd.SetBackColor(LCD_COLOR_WHITE);
                    lcd.DisplayStringAt(0, LINE(16),(unsigned char *)"LASER AVG : ",LEFT_MODE);
                    lcd.DisplayStringAt(200, LINE(16),(unsigned char *)message3, LEFT_MODE);
                }
                break;

            case RECEPTION_COULEUR:
                if (blocage_balise==0) {
                    couleur1=msgRxBuffer[FIFO_lecture].data[0];
                    couleur2=msgRxBuffer[FIFO_lecture].data[1];
                    couleur3=msgRxBuffer[FIFO_lecture].data[2];

                    /*lcd.DisplayStringAt(0,LINE(16),(unsigned char *)couleur1+'0',LEFT_MODE);
                    lcd.DisplayStringAt(0,LINE(16+1),(unsigned char *)couleur2+'0',LEFT_MODE);
                    lcd.DisplayStringAt(0,LINE(16+2),(unsigned char *)couleur3+'0',LEFT_MODE);*/
                }

                break;

            case NO_BLOC: //il n'y a pas de bloc, on saute les étapes liées à l'attrape bloc
                actual_instruction = instruction.nextLineError;
                gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
                /*waitingAckID_FIN=0;
                waitingAckFrom_FIN=0;*/
                SendRawId(0x40);
                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,centaine=0;
    centaine=nombre/100;
    dizaine = nombre/10;
    unite = nombre-(10*dizaine);
    print_segment(unite,-50);
    print_segment(dizaine,100);
    if(centaine!=0) {
        print_segment(centaine,350);
    }

}


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

short recalageAngulaireCapteur(void)
{
    unsigned char nombresDeMesuresAuxTelemetresQuiSontCoherentes         = 0;
    unsigned int  moyennageTelemetre                                    = 0;
    unsigned short angleAvant = 0;
    unsigned short angleArriere = 0;
    unsigned short orientationArrondie = 0;

    unsigned short position_avant_gauche=0;
    unsigned short position_avant_droite=0;
    unsigned short position_arriere_gauche=0;
    unsigned short position_arriere_droite=0;

    if(theta_robot >= 450 && theta_robot <= 1350)
        orientationArrondie = 90;
    else if(theta_robot <= -450 && theta_robot >= -1350)
        orientationArrondie = 270;
    else if(theta_robot <= 450 && theta_robot >= -450)
        orientationArrondie = 0;
    else if(theta_robot >= 1350 && theta_robot <= -1350)
        orientationArrondie = 180;

    // Calcul de position pour faire la vérification de cohérence
    if(orientationArrondie == 90 || orientationArrondie == 270) {
        position_avant_gauche   = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?3000:0) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?-1:1)*telemetreDistance_avant_gauche;
        position_avant_droite   = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?3000:0) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?-1:1)*telemetreDistance_avant_droite;
        position_arriere_gauche = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?0:3000) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?1:-1)*telemetreDistance_arriere_gauche;
        position_arriere_droite = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?0:3000) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?1:-1)*telemetreDistance_arriere_droite;

    } else if(orientationArrondie == 0 || orientationArrondie == 180) {
        position_avant_gauche   = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_avant_gauche;
        position_avant_droite   = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_avant_droite;
        position_arriere_gauche = ((theta_robot < 900 && theta_robot > -900)?0:2000) + ((theta_robot < 900 && theta_robot > -900)?1:-1)*telemetreDistance_arriere_gauche;
        position_arriere_droite = ((theta_robot < 900 && theta_robot > -900)?0:2000) + ((theta_robot < 900 && theta_robot > -900)?1:-1)*telemetreDistance_arriere_droite;
    }


    if(orientationArrondie == 90 || orientationArrondie == 270) { // Si il est en axe Y
        if(position_arriere_droite >= y_robot-instruction.arg1 && position_arriere_droite <= y_robot+instruction.arg1) { // Et que les mesures sont cohérentes
            if(position_arriere_gauche >= y_robot-instruction.arg1 && position_arriere_gauche <= y_robot+instruction.arg1) {
                if(telemetreDistance_arriere_droite > telemetreDistance_arriere_gauche)
                    angleArriere =900+(1800 * atan2((double)(telemetreDistance_arriere_droite-telemetreDistance_arriere_gauche), (double)ESPACE_INTER_TELEMETRE ))/M_PI;
                else
                    angleArriere =(1800 * atan2( (double) ESPACE_INTER_TELEMETRE,(double) (telemetreDistance_arriere_gauche-telemetreDistance_arriere_droite) ))/M_PI;

                nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
                moyennageTelemetre += angleArriere;
            }
        }
    } else if(orientationArrondie == 0 || orientationArrondie == 180) { // Si il est en axe X
        if(position_arriere_droite >= x_robot-instruction.arg1 && position_arriere_droite <= x_robot+instruction.arg1) { // Et que les mesures sont cohérentes
            if(position_arriere_gauche >= x_robot-instruction.arg1 && position_arriere_gauche <= x_robot+instruction.arg1) {
                if(telemetreDistance_arriere_droite > telemetreDistance_arriere_gauche)
                    angleArriere =900+(1800 * atan2( (double) (telemetreDistance_arriere_droite-telemetreDistance_arriere_gauche), (double) ESPACE_INTER_TELEMETRE ))/M_PI;
                else
                    angleArriere =(1800 * atan2( (double) ESPACE_INTER_TELEMETRE,(double) (telemetreDistance_arriere_gauche-telemetreDistance_arriere_droite) ))/M_PI;

                nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
                moyennageTelemetre += angleArriere;
            }
        }
    }

    if(orientationArrondie == 90 || orientationArrondie == 270) { // Si il est en axe Y
        if(position_avant_droite >= y_robot-instruction.arg1 && position_avant_droite <= y_robot+instruction.arg1) { // Et que les mesures sont cohérentes
            if(position_avant_gauche >= y_robot-instruction.arg1 && position_avant_gauche <= y_robot+instruction.arg1) {
                if(telemetreDistance_avant_droite > telemetreDistance_avant_gauche)
                    angleAvant = (1800 * atan2( (double) ESPACE_INTER_TELEMETRE,(double) (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) ))/M_PI;
                else
                    angleAvant = 900 + (1800 * atan2( (double)( telemetreDistance_avant_gauche-telemetreDistance_avant_droite),(double) ESPACE_INTER_TELEMETRE ))/M_PI;

                nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
                moyennageTelemetre += angleAvant;
            }
        }
    } else if(orientationArrondie == 0 || orientationArrondie == 180) { // Si il est en axe X
        if(position_avant_droite >= x_robot-instruction.arg1 && position_avant_droite <= x_robot+instruction.arg1) { // Et que les mesures sont cohérentes
            if(position_avant_gauche >= x_robot-instruction.arg1 && position_avant_gauche <= x_robot+instruction.arg1) {
                if(telemetreDistance_avant_droite > telemetreDistance_avant_gauche)
                    angleAvant = (1800 * atan2((double) ESPACE_INTER_TELEMETRE, (double) (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) ))/M_PI;
                else
                    angleAvant = 900 + (1800 * atan2( (double) (telemetreDistance_avant_gauche-telemetreDistance_avant_droite),(double) ESPACE_INTER_TELEMETRE ))/M_PI;

                nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
                moyennageTelemetre += angleAvant;
            }
        }
    }

    angleRecalage = moyennageTelemetre/nombresDeMesuresAuxTelemetresQuiSontCoherentes;



    if(nombresDeMesuresAuxTelemetresQuiSontCoherentes) {
        if(orientationArrondie == 0) {
            angleRecalage -= 900;

            /*if(telemetreDistance_avant_droite > telemetreDistance_avant_gauche)
                distanceRecalage = *);
            else
                distanceRecalage = 900 + (1800 * atan( (double)( (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) / ESPACE_INTER_TELEMETRE )))/M_PI;*/
        } else if(orientationArrondie == 90) {
            angleRecalage += 0;
        } else if(orientationArrondie == 180) {
            angleRecalage += 900;
        } else if(orientationArrondie == 270) {
            angleRecalage += 1800;
        }
    }

    return (nombresDeMesuresAuxTelemetresQuiSontCoherentes && (angleAvant-angleArriere<80 && angleAvant-angleArriere>-80)) ? angleRecalage : theta_robot;
}

short recalageDistanceX(void)
{
    unsigned char nombresDeMesuresAuxTelemetresQuiSontCoherentes         = 0;
    unsigned int  moyennageTelemetre                                    = 0;

    telemetreDistance_avant_gauche   = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_avant_gauche;
    telemetreDistance_avant_droite   = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_avant_droite;
    telemetreDistance_arriere_gauche = ((theta_robot < 900 && theta_robot > -900)?0:2000) + ((theta_robot < 900 && theta_robot > -900)?1:-1)*telemetreDistance_arriere_gauche;
    telemetreDistance_arriere_droite = ((theta_robot < 900 && theta_robot > -900)?0:2000) + ((theta_robot < 900 && theta_robot > -900)?1:-1)*telemetreDistance_arriere_droite;

    if(telemetreDistance_avant_gauche >= x_robot-instruction.arg1 && telemetreDistance_avant_gauche <= x_robot+instruction.arg1) {
        nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
        moyennageTelemetre += telemetreDistance_avant_gauche;
    }
    if(telemetreDistance_avant_droite >= x_robot-instruction.arg1 && telemetreDistance_avant_droite <= x_robot+instruction.arg1) {
        nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
        moyennageTelemetre += telemetreDistance_avant_droite;
    }
    if(telemetreDistance_arriere_gauche >= x_robot-instruction.arg1 && telemetreDistance_arriere_gauche <= x_robot+instruction.arg1) {
        nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
        moyennageTelemetre += telemetreDistance_arriere_gauche;
    }
    if(telemetreDistance_arriere_droite >= x_robot-instruction.arg1 && telemetreDistance_arriere_droite <= x_robot+instruction.arg1) {
        nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
        moyennageTelemetre += telemetreDistance_arriere_droite;
    }

    moyennageTelemetre /= nombresDeMesuresAuxTelemetresQuiSontCoherentes;

    return (nombresDeMesuresAuxTelemetresQuiSontCoherentes)? moyennageTelemetre : x_robot; //SetOdometrie(ODOMETRIE_SMALL_POSITION, moyennageTelemetre, y_robot, theta_robot);
}

short recalageDistanceY(void)
{
    unsigned char   nombresDeMesuresAuxTelemetresQuiSontCoherentes        = 0;
    unsigned int    moyennageTelemetre                                    = 0;

    telemetreDistance_avant_gauche   = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?3000:0) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?-1:1)*telemetreDistance_avant_gauche;
    telemetreDistance_avant_droite   = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?3000:0) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?-1:1)*telemetreDistance_avant_droite;
    telemetreDistance_arriere_gauche = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?0:3000) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?1:-1)*telemetreDistance_arriere_gauche;
    telemetreDistance_arriere_droite = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?0:3000) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?1:-1)*telemetreDistance_arriere_droite;

    if(telemetreDistance_avant_gauche >= y_robot-instruction.arg1 && telemetreDistance_avant_gauche <= y_robot+instruction.arg1) {
        nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
        moyennageTelemetre += telemetreDistance_avant_gauche;
    }
    if(telemetreDistance_avant_droite >= y_robot-instruction.arg1 && telemetreDistance_avant_droite <= y_robot+instruction.arg1) {
        nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
        moyennageTelemetre += telemetreDistance_avant_droite;
    }
    if(telemetreDistance_arriere_gauche >= y_robot-instruction.arg1 && telemetreDistance_arriere_gauche <= y_robot+instruction.arg1) {
        nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
        moyennageTelemetre += telemetreDistance_arriere_gauche;
    }
    if(telemetreDistance_arriere_droite >= y_robot-instruction.arg1 && telemetreDistance_arriere_droite <= y_robot+instruction.arg1) {
        nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
        moyennageTelemetre += telemetreDistance_arriere_droite;
    }

    moyennageTelemetre /= nombresDeMesuresAuxTelemetresQuiSontCoherentes;

    return (nombresDeMesuresAuxTelemetresQuiSontCoherentes)? moyennageTelemetre : y_robot ; // SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, moyennageTelemetre, theta_robot);
}