Diff: Strategie/Strategie.cpp

Revision:

0:ad97421fb1fb

Child:

1:116040d14164

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Strategie/Strategie.cpp	Wed Apr 13 22:04:54 2016 +0000
@@ -0,0 +1,435 @@
+#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;
+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
+
+unsigned short id_check[NOMBRE_CARTES]= {CHECK_BALISE,CHECK_MOTEUR};
+unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_BALISE,ALIVE_MOTEUR};
+unsigned char checkCurrent = 0;
+unsigned char countAliveCard = 0; 
+
+
+void chronometre_ISR (void)
+{
+    SendRawId(ASSERVISSEMENT_STOP);//On stope les moteurs
+    SendRawId(GLOBAL_GAME_END);//Indication fin de match
+    gameTimer.stop();//Arret du timer
+
+#if ROBOT_TYPE == ROBOT_BIG
+    doFunnyAction();
+#endif
+    
+    gameEtat = ETAT_END_LOOP;
+}
+
+void automate_process(void)
+{
+    static struct S_Instruction instruction;
+    signed char localData1 = 0;
+    signed short localData2 = 0;
+    unsigned short localData3 = 0;
+    signed short localData4 = 0;
+    
+    if(gameTimer.read_ms() >= 88000) {//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);
+    }
+    
+    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 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_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++;
+                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 {
+                    checkCurrent++;
+                    gameEtat = ETAT_CHECK_CARTES;
+                }
+            } else if(cartesCheker.read_ms () > 100) {
+                cartesCheker.stop();
+                if(screenChecktry >=3) {
+                    screenChecktry = 0;
+                    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 {
+                        checkCurrent++;
+                        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_START:
+            //On charge la liste des instructions
+            strcpy(cheminFileStart,"/local/test.txt");//On ouvre le fichier test.txt
+            loadAllInstruction();//Mise en cache de toute les instructions
+            fakeJack.reset();//Utiliser pour simuler le jack
+            fakeJack.start();//Utiliser pour simuler le jack
+            gameEtat = ETAT_GAME_WAIT_FOR_JACK;
+            SendRawId(ECRAN_ACK_START_MATCH);
+            tactile_printf("Attente du JACK.");
+        break;
+        case ETAT_GAME_WAIT_FOR_JACK:
+            //TODO Attendre le jack
+            if(fakeJack.read_ms() > 3000) {//Utiliser pour simuler le jack
+                fakeJack.stop();
+                gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
+                gameTimer.reset();
+                gameTimer.start();
+                chronoEnd.attach(&chronometre_ISR,90);
+            }
+        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;
+                //actual_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:
+                    //TODO - mettre l'ACK à jour
+                    if(instruction.nextActionType == ENCHAINEMENT) {
+                        //TODO - flag dans le trame pour indiquer l'enchainement    
+                    }
+                break;
+                case MV_LINE:
+                    waitingAckID = ASSERVISSEMENT_RECALAGE;
+                    waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+                    if(instruction.nextActionType == ENCHAINEMENT) {
+                        //TODO - flag dans le trame pour indiquer l'enchainement  
+                        
+                    }
+                break;
+                case MV_TURN:
+                    if(instruction.direction == RELATIVE) {
+                        localData2 = instruction.arg3;
+                    } else {
+                        if(abs(instruction.arg3 - theta_robot) > 180) {
+                            localData2 = 360 - instruction.arg3 - theta_robot;
+                        } else {
+                            localData2 = instruction.arg3 - theta_robot;
+                        }
+                    }
+                    Rotate(localData2);
+                    waitingAckID = ASSERVISSEMENT_ROTATION;
+                    waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+                break;
+                case MV_XYT:
+                    if(instruction.direction == BACKWARD) {
+                        localData1 = -1;
+                    } else {
+                        localData1 = 1;
+                    }
+                    GoToPosition(instruction.arg1,instruction.arg2,instruction.arg3,localData1);
+                    waitingAckID = ASSERVISSEMENT_XYT;
+                    waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+                break;
+                case MV_RECALAGE:
+                    waitingAckID = ASSERVISSEMENT_RECALAGE;
+                    waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+                    //TODO - mettre l'ACK à jour
+                break;
+                case ACTION:
+                    if(doAction(instruction.arg1,instruction.arg2,instruction.arg3)) {
+                        //L'action est spécifique
+                    } else {
+                        //C'est un AX12 qu'il faut bouger
+                    }
+                
+                    //TODO - mettre l'ACK à jour
+                    if(instruction.nextActionType == ENCHAINEMENT) {
+                        //TODO - flag dans le trame pour indiquer l'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();
+            } 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_TIME) {
+                        gameEtat = ETAT_GAME_JUMP_TIME;
+                        cartesCheker.reset();//On reset le timeOut
+                        cartesCheker.start();
+                    } else if(instruction.jumpAction == JUMP_POSITION) {
+                        gameEtat = ETAT_GAME_JUMP_POSITION;
+                    } else {
+                        //ERROR   
+                    }
+                } 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:
+                            //TODO 
+                        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_END:
+           
+            gameEtat = ETAT_END_LOOP;
+        break;
+        case ETAT_END_LOOP:
+            //Rien, on tourne en rond
+        break;
+        default:
+        
+        break;
+    }
+}
+
+void canProcessRx(void)
+{
+    static signed char FIFO_occupation=0,FIFO_max_occupation=0;
+
+    FIFO_occupation=FIFO_ecriture-FIFO_lecture;
+    if(FIFO_occupation<0)
+        FIFO_occupation=FIFO_occupation+SIZE_FIFO;
+    if(FIFO_max_occupation<FIFO_occupation)
+        FIFO_max_occupation=FIFO_occupation;
+    if(FIFO_occupation!=0) {
+        
+        switch(msgRxBuffer[FIFO_lecture].id) {
+            case ASSERVISSEMENT_STOP:
+                
+            break;
+            case ALIVE_BALISE:
+            case ALIVE_MOTEUR:
+            case ALIVE_IHM:
+            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 && (msgRxBuffer[FIFO_lecture].data[0]|((unsigned int)(msgRxBuffer[FIFO_lecture].data[1])<<8) == waitingAckID)) {
+                    waitingAckFrom = 0;
+                    waitingAckID = 0;
+                }
+            break;
+#if ROBOT_TYPE == 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_START;
+                }
+            break;
+            case 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:
+                AX12_processChange();
+            break;
+        }        
+        
+        FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
+    }
+}