pour trouver l'automate de reception can
Fork of CRAC-Strat_copy by
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; + } +}