CRAC Team
homologation gros robot et test avec les ack de la carte a tout faire
Fork of
CRAC-Strat_2017_HOMOLOGATION_PETIT_ROBOT
by 
CRAC Team
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;
+ }
+}