File content as of revision 12:14729d584500:

#include "Strategie.h"

E_stratGameEtat     gameEtat  = ETAT_CHECK_CARTE_SCREEN;
E_stratGameEtat     lastEtat  = ETAT_CHECK_CARTE_SCREEN;
unsigned char screenChecktry = 0;
Timer cartesCheker;//Le timer pour le timeout de la vérification des cartes
Timer fakeJack;
Timer gameTimer;
Timer debugetatTimer;
Timer timeoutWarning;
Timer timeoutWarningWaitEnd;
Timeout chronoEnd;//permet d'envoyer la trame CAN pour la fin 

unsigned short waitingAckID = 0;//L'id du ack attendu
unsigned short waitingAckFrom = 0;//La provenance du ack attendu

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

#ifdef ROBOT_BIG
unsigned short id_check[NOMBRE_CARTES]= {CHECK_BALISE,CHECK_MOTEUR,CHECK_ACTIONNEURS,CHECK_AX12,CHECK_POMPES};
unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_BALISE,ALIVE_MOTEUR,ALIVE_ACTIONNEURS,ALIVE_AX12,ALIVE_POMPES};
InterruptIn jack(p25); //  entrée analogique en interruption pour le jack
#else
unsigned short id_check[NOMBRE_CARTES]= {CHECK_BALISE,CHECK_MOTEUR,CHECK_ACTIONNEURS};
unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_BALISE,ALIVE_MOTEUR,ALIVE_ACTIONNEURS};
InterruptIn jack(p24); //  entrée analogique en interruption pour le jack
#endif
unsigned char checkCurrent = 0;
unsigned char countAliveCard = 0; 

unsigned char InversStrat = 1;//Si à 1, indique que l'on part de l'autre cote de la table(inversion des Y)

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

unsigned char ingnorBaliseOnce = 0;

/****************************************************************************************/
/* 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
    gameTimer.stop();//Arret du timer

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

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

/****************************************************************************************/
/* FUNCTION NAME: automate_process                                                      */
/* DESCRIPTION  : Automate de gestion de la stratégie du robot                          */
/****************************************************************************************/
void automate_process(void)
{
    static struct S_Instruction instruction;
    static unsigned char AX12_enchainement = 0;
    static unsigned char MV_enchainement = 0;
    signed char localData1 = 0;
    signed short localData2 = 0;
    unsigned short localData3 = 0;
    signed short localData4 = 0;
    unsigned char localData5 = 0;
    
    if(gameTimer.read_ms() >= 89000) {//Fin du match (On autorise 2s pour déposer des éléments
        gameTimer.stop();
        gameTimer.reset();
        gameEtat = ETAT_END;//Fin du temps
    }
    
    if(lastEtat != gameEtat || debugetatTimer.read_ms() >= 1000) {
        lastEtat = gameEtat;
        debugetatTimer.reset();
        sendStratEtat((unsigned char)gameEtat, (unsigned char)actual_instruction);
    }
    
    switch(gameEtat)
    {
        case ETAT_CHECK_CARTE_SCREEN:
            /*
            Verification de l'état de la carte ecran
            */
            waitingAckFrom = ALIVE_IHM;//On indique que l'on attend un ack de la carte IHM
            SendRawId(CHECK_IHM);//On demande à la carte IHM d'insiquer ç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_CARTE_SCREEN_WAIT_ACK;
            
        break;
        case ETAT_CHECK_CARTE_SCREEN_WAIT_ACK:
            /*
            Attente du ALIVE de la carte écran.
            
            Si la carte ne répond pas apres 10ms, on retoune dans l'etat ETAT_CHECK_CARTE_SCREEN
            maximum 3 tentatives
            Si pas de réponse, clignotement de toutes les leds possible
            */
            if(waitingAckFrom == 0) {//C'est bon la carte est en ligne
                cartesCheker.stop();
                screenChecktry = 0;
                gameEtat = ETAT_CHECK_CARTES;
            } else if(cartesCheker.read_ms () > 100) {
                cartesCheker.stop();
                if(screenChecktry >=3) {
                    errorLoop();//Erreur La carte IHM n'est pas en ligne
                } else {
                    gameEtat = ETAT_CHECK_CARTE_SCREEN;
                }
            }
        break;
        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);//On dit à l'IHM que toutes les cartes sont en ligne
                        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;
                            SendRawId(ECRAN_ALL_CHECK);//On dit à l'IHM que toutes les cartes sont en ligne
                        } else {
                            gameEtat = ETAT_WAIT_FORCE;//Passage en attente de forçage du lancement
                            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
            */
        break;
        case ETAT_GAME_INIT:
            //On charge la liste des instructions
            loadAllInstruction();//Mise en cache de toute les instructions
            gameEtat = ETAT_GAME_WAIT_FOR_JACK;
            SendRawId(ECRAN_ACK_START_MATCH);
            tactile_printf("Attente du JACK.");
            setAsservissementEtat(1);//On réactive l'asservissement
            jack.mode(PullDown); // désactivation de la résistance interne du jack
            jack.fall(&jack_ISR); // création de l'interrupt attachée au changement d'état (front descendant) sur le jack
         
#ifdef ROBOT_BIG //le gros robot n'a pas de recalage bordure pour ce placer au début, on lui envoit donc ça position
            localData2 = POSITION_DEBUT_T;
            localData3 = POSITION_DEBUT_Y;
            if(InversStrat == 1) {
                localData2 = -localData2;//Inversion theta
                localData3 = 3000 - POSITION_DEBUT_Y;//Inversion du Y
            }
            SetOdometrie(ODOMETRIE_BIG_POSITION, POSITION_DEBUT_X,localData3,localData2);
#endif /*
            SetOdometrie(ODOMETRIE_SMALL_POSITION, POSITION_DEBUT_X,POSITION_DEBUT_Y,POSITION_DEBUT_T);
#endif*/
        break;
        case ETAT_GAME_WAIT_FOR_JACK:
            //On attend le jack
        break;
        case ETAT_GAME_START:
            chronoEnd.attach(&chronometre_ISR,90);//On lance le chrono de 90s
            gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
            gameTimer.reset();
            gameTimer.start();
            jack.fall(NULL);//On désactive l'interruption du jack
            SendRawId(GLOBAL_START);
            tactile_printf("Start");//Pas vraiment util 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;
        break;
        case ETAT_GAME_PROCESS_INSTRUCTION:
            /*
            Traitement de l'instruction, envoie de la trame CAN
            */
            //debug_Instruction(instruction);
            switch(instruction.order)
            {
                case MV_COURBURE://C'est un rayon de 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);
                    if(InversStrat == 1)
                    {
                        localData1 = -localData1;//Inversion de la direction
                    }
                    BendRadius(instruction.arg1, instruction.arg3, localData1, localData5);
                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);
                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;
                        
                        if(InversStrat == 1) {
                            localData2 = -localData2;
                        }
                        
                        localData2 = (localData2 - theta_robot)%3600;
                        if(localData2 > 1800) {
                            localData2 = localData2-3600;
                        }
                        
                    }
                    
                    Rotate(localData2);
                    waitingAckID = ASSERVISSEMENT_ROTATION;
                    waitingAckFrom = ACKNOWLEDGE_MOTEUR;
                break;
                case MV_XYT:
                    if(instruction.direction == BACKWARD) {
                        localData1 = -1;
                    } else {
                        localData1 = 1;
                    }
                    
                    if(InversStrat == 1) {
                        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;
                break;
                case MV_RECALAGE:
                    waitingAckID = ASSERVISSEMENT_RECALAGE;
                    waitingAckFrom = ACKNOWLEDGE_MOTEUR;
                    instruction.nextActionType = WAIT;
                    localData2 = (((instruction.direction == FORWARD)?1:-1)*3000);//On indique une distance de 3000 pour etre sur que le robot va ce recaler
                    
                    if(instruction.precision == RECALAGE_Y) {
                        localData5 = 2;
                        if(InversStrat == 1) {
                            localData3 = 3000 - instruction.arg1;//Inversion du Y
                        } else {
                            localData3 = instruction.arg1;
                        }
                    } else {
                        localData5 = 1;
                        localData3 = instruction.arg1;
                    }
                    
                    GoStraight(localData2, localData5, localData3, 0);
                break;
                case ACTION:
                    waitingAckFrom = 0;
                    waitingAckID = 0;
                    if(doAction(instruction.arg1,instruction.arg2,instruction.arg3)) {
                        //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_END_INSTRUCTION;
                        }
                        return;
                    } 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
                actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
                gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//C'est un enchainement, on charge directement l'instruction suivante
            }
            
        break;
        case ETAT_GAME_WAIT_ACK:
            /*
            Attente de l'ack de l'instruction
            */
            if(waitingAckID == 0 && waitingAckFrom ==0) {//Les ack ont été reset, c'est bon on continu
            //if(true) {
                cartesCheker.stop();
                if(instruction.nextActionType == JUMP) {
                    if(instruction.jumpAction == JUMP_POSITION) {
                        gameEtat = ETAT_GAME_JUMP_POSITION;
                    } else {//Pour eviter les erreurs, on dit que c'est par défaut un jump time
                        gameEtat = ETAT_GAME_JUMP_TIME;
                        cartesCheker.reset();//On reset le timeOut
                        cartesCheker.start();  
                    }
                } else if(instruction.nextActionType == WAIT) {
                    gameEtat = ETAT_GAME_WAIT_END_INSTRUCTION;
                     switch(instruction.order)
                    {
                        case MV_COURBURE:
                            waitingAckID = ASSERVISSEMENT_COURBURE;
                            waitingAckFrom = INSTRUCTION_END_MOTEUR;
                        break;
                        case MV_LINE:
                            waitingAckID = ASSERVISSEMENT_RECALAGE;
                            waitingAckFrom = INSTRUCTION_END_MOTEUR;
                        break;
                        case MV_TURN:
                            waitingAckID = ASSERVISSEMENT_ROTATION;
                            waitingAckFrom = INSTRUCTION_END_MOTEUR;
                        break;
                        case MV_XYT:
                            waitingAckID = ASSERVISSEMENT_XYT;
                            waitingAckFrom = INSTRUCTION_END_MOTEUR;
                        break;
                        case MV_RECALAGE:
                            waitingAckID = ASSERVISSEMENT_RECALAGE;
                            waitingAckFrom = INSTRUCTION_END_MOTEUR;
                        break;
                        case ACTION:
                            waitingAckFrom = 0;//SERVO_AX12_DONE;
                            waitingAckID = 0;//instruction.arg1;
                        break;
                        default:
                        break;
                    }   
                } else {
                    gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
                    actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
                }
            } else if(cartesCheker.read_ms () > 50){
                cartesCheker.stop();
                if(screenChecktry >=2) {//La carte n'a pas reçus l'information, on passe à l'instruction d'erreur
                    actual_instruction = instruction.nextLineError;
                    gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
                } else {
                    gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;//On retourne dans l'etat d'envois de l'instruction
                }
            }
        break;
        
        case ETAT_GAME_JUMP_TIME:
            if(cartesCheker.read_ms () >= instruction.JumpTimeOrX) {
                cartesCheker.stop();//On arrete le timer
                actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
                gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante
            }
        break;
        case ETAT_GAME_JUMP_POSITION:
            
        break;
        case ETAT_GAME_WAIT_END_INSTRUCTION:
            if(waitingAckID == 0 && waitingAckFrom ==0) {//On attend que la carte nous indique que l'instruction est terminée
                actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
                gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante
            }
        break;
        
        
        case ETAT_WARNING_TIMEOUT://Attente de la trame fin de danger ou du timeout de 2s
            if(timeoutWarning.read_ms() >= BALISE_TIMEOUT)//ça fait plus de 2s, il faut changer de stratégie
            {
                gameEtat = ETAT_WARNING_SWITCH_STRATEGIE;
            }
        break;
        case ETAT_WARING_END_BALISE_WAIT://Attente d'une seconde apres la fin d'un End Balise pour etre sur que c'est bon
            if(timeoutWarningWaitEnd.read_ms() >= 1000) {//c'est bon, on repart
                //actual_instruction = instruction.nextLineError;
                gameEtat = ETAT_WARNING_END_LAST_INSTRUCTION;
            }
        break;
        case ETAT_WARNING_END_LAST_INSTRUCTION://trouver le meilleur moyen de reprendre l'instruction en cours
#ifdef ROBOT_BIG
            actual_instruction = 2;//Modification directe... c'est pas bien mais ça marchait pour le match 5
            gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
#else
            actual_instruction = instruction.nextLineError;
            gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; 
#endif
        break;
        case ETAT_WARNING_SWITCH_STRATEGIE://Si à la fin du timeout il y a toujours un robot, passer à l'instruction d'erreur
            actual_instruction = instruction.nextLineError;
            gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
            ingnorBaliseOnce = 1;
        break;
        
        
        
        case ETAT_END:
           
            gameEtat = ETAT_END_LOOP;
        break;
        case ETAT_END_LOOP:
            //Rien, on tourne en rond
        break;
        default:
        
        break;
    }
}

/****************************************************************************************/
/* FUNCTION NAME: canProcessRx                                                          */
/* DESCRIPTION  : Fonction de traitement des messages CAN                               */
/****************************************************************************************/
void canProcessRx(void)
{
    static signed char FIFO_occupation=0,FIFO_max_occupation=0;
    CANMessage msgTx=CANMessage();
    FIFO_occupation=FIFO_ecriture-FIFO_lecture;
    if(FIFO_occupation<0)
        FIFO_occupation=FIFO_occupation+SIZE_FIFO;
    if(FIFO_max_occupation<FIFO_occupation)
        FIFO_max_occupation=FIFO_occupation;
    if(FIFO_occupation!=0) {
        
        switch(msgRxBuffer[FIFO_lecture].id) {
            case DEBUG_FAKE_JAKE://Permet de lancer le match à distance
                if(gameEtat == ETAT_GAME_WAIT_FOR_JACK) {
                    gameEtat = ETAT_GAME_START;
                }
            break;
            
            case ALIVE_BALISE:
            case ALIVE_MOTEUR:
            case ALIVE_IHM:
            case ALIVE_ACTIONNEURS:
            case ALIVE_POMPES:
            case ALIVE_AX12:
            case ECRAN_ALL_CHECK:
                if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id) {
                    waitingAckFrom = 0;//C'est la bonne carte qui indique qu'elle est en ligne
                }
            break;
            
            case ACKNOWLEDGE_BALISE:
            case ACKNOWLEDGE_MOTEUR:
            case ACKNOWLEDGE_IHM:
            case INSTRUCTION_END_BALISE:
            case INSTRUCTION_END_MOTEUR:
            case INSTRUCTION_END_IHM:
                if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id && ((unsigned short)msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8) == waitingAckID)) {
                    waitingAckFrom = 0;
                    waitingAckID = 0;
                }
            break;
#ifdef ROBOT_BIG
            case ODOMETRIE_BIG_POSITION:
#else
            case ODOMETRIE_SMALL_POSITION:
#endif
                x_robot=msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8);
                y_robot=msgRxBuffer[FIFO_lecture].data[2]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[3])<<8);
                theta_robot=msgRxBuffer[FIFO_lecture].data[4]|((signed short)(msgRxBuffer[FIFO_lecture].data[5])<<8);
            break;
            
            case ECRAN_START_MATCH:
                if(gameEtat == ETAT_CONFIG) {
                    gameEtat = ETAT_GAME_INIT;
                }
            break;
            case SERVO_AX12_SETGOAL:
                //SendAck(0x114, SERVO_AX12_SETGOAL);
                if(AX12_isLocal(msgRxBuffer[FIFO_lecture].data[0]))
                    AX12_setGoal(msgRxBuffer[FIFO_lecture].data[0], msgRxBuffer[FIFO_lecture].data[1]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[2])<<8), msgRxBuffer[FIFO_lecture].data[3]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[4])<<8));
                  
            break;
            case SERVO_AX12_PROCESS:
                //SendAck(0x114, SERVO_AX12_PROCESS);
                AX12_processChange(1);
            break;
            case SERVO_AX12_DONE:
                //SendAck(0x114, SERVO_AX12_DONE);
                AX12_notifyCANEnd(((unsigned short)(msgRxBuffer[FIFO_lecture].data[0])));
            break;
            case ECRAN_CHOICE_COLOR://Choix de la couleur
                if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config
                    if(msgRxBuffer[FIFO_lecture].data[0] == 0)
                        InversStrat = 0;//Pas d'inversion de la couleur
                    else
                        InversStrat = 1;//Inversion de la couleur
                        
                    msgTx.id=ECRAN_ACK_COLOR; // tx ack de la couleur
                    msgTx.len=1;
                    msgTx.format=CANStandard;
                    msgTx.type=CANData;
                    // couleur sur 1 octet
                    msgTx.data[0]=msgRxBuffer[FIFO_lecture].data[0];
                    can1.write(msgTx);
                    
                }
            break;
            case ECRAN_CHOICE_STRAT://Choix du fichier de stratégie à utiliser
                if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config
                    msgTx.id=ECRAN_ACK_STRAT; // tx ack de la couleur
                    msgTx.len=1;
                    msgTx.format=CANStandard;
                    msgTx.type=CANData;
                    if(SelectStrategy(msgRxBuffer[FIFO_lecture].data[0])) {
                        // id de la stratégie sur 1 octet
                        msgTx.data[0]=msgRxBuffer[FIFO_lecture].data[0];
                    } else {
                        //erreur sur 1 octet
                        msgTx.data[0]=0;
                    }
                    can1.write(msgTx);
                    wait_ms(10);
                    setAsservissementEtat(0);//Désactivation de l'asservissement pour repositionner le robot dans le zone de départ
                    tactile_printf("Strat %d, Asser desactive",msgTx.data[0]);
                }
            break;
            case BALISE_STOP:
                SendAck(ACKNOWLEDGE_BALISE, BALISE_STOP);
                
                if(needToStop() != 0 && ingnorBaliseOnce ==0) {
                    if(gameEtat > ETAT_GAME_START && gameEtat != ETAT_WARNING_TIMEOUT)
                    {
                        SendRawId(ASSERVISSEMENT_STOP);
                        //while(1);
                        gameEtat = ETAT_WARNING_TIMEOUT;
                        if(gameEtat != ETAT_WARING_END_BALISE_WAIT) {
                            timeoutWarning.reset();
                            timeoutWarning.start();//Reset du timer utiliser par le timeout
                        }
                    }
                }
                ingnorBaliseOnce = 0;
            break;
            case BALISE_END_DANGER:
                if(gameEtat == ETAT_WARNING_TIMEOUT) {
                    timeoutWarningWaitEnd.reset();
                    timeoutWarningWaitEnd.start();
                    gameEtat = ETAT_WARING_END_BALISE_WAIT;
                }
            break;
            
            case ECRAN_CHOICE_START_ACTION:
                if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config
                    if(msgRxBuffer[FIFO_lecture].data[0] == 1) {
                        runRobotTest();
                    } else {
                        initRobotActionneur();
                    }
                    wait_ms(500);
                    SendRawId(ECRAN_ACK_CHOICE_START_ACTION);
                }
            break;
        }        
        
        FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
    }
}