Diff: Strategie/Strategie.cpp

Revision:

14:c8fc06c4887f

Parent:

12:14729d584500

Child:

15:c2fc239e85df

diff -r 93edbb03a8c6 -r c8fc06c4887f Strategie/Strategie.cpp
--- a/Strategie/Strategie.cpp	Tue Jan 31 16:00:33 2017 +0000
+++ b/Strategie/Strategie.cpp	Fri Mar 31 16:20:26 2017 +0000
@@ -1,4 +1,4 @@
-#include "Strategie.h"
+ #include "Strategie.h"
 
 E_stratGameEtat     gameEtat  = ETAT_CHECK_CARTE_SCREEN;
 E_stratGameEtat     lastEtat  = ETAT_CHECK_CARTE_SCREEN;
@@ -13,6 +13,7 @@
 
 unsigned short waitingAckID = 0;//L'id du ack attendu
 unsigned short waitingAckFrom = 0;//La provenance du ack attendu
+char modeTelemetre; // Si à 1, indique que l'on attend une reponse du telemetre 
 
 signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
 
@@ -21,8 +22,12 @@
 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};
+//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};
+
+unsigned short id_check[NOMBRE_CARTES]= {CHECK_MOTEUR};
+unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_MOTEUR};
+
 InterruptIn jack(p25); //  entrée analogique en interruption pour le jack
 #else
 unsigned short id_check[NOMBRE_CARTES]= {CHECK_BALISE,CHECK_MOTEUR,CHECK_ACTIONNEURS};
@@ -34,10 +39,13 @@
 
 unsigned char InversStrat = 1;//Si à 1, indique que l'on part de l'autre cote de la table(inversion des Y)
 
+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;
 
+
 /****************************************************************************************/
 /* FUNCTION NAME: chronometre_ISR                                                       */
 /* DESCRIPTION  : Interruption à la fin des 90s du match                                */
@@ -80,7 +88,7 @@
     signed char localData1 = 0;
     signed short localData2 = 0;
     unsigned short localData3 = 0;
-    signed short localData4 = 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
@@ -109,6 +117,8 @@
             cartesCheker.start();//On lance le timer pour le timeout
             gameEtat = ETAT_CHECK_CARTE_SCREEN_WAIT_ACK;
             
+            //gameEtat = ETAT_GAME_START;
+            
         break;
         case ETAT_CHECK_CARTE_SCREEN_WAIT_ACK:
             /*
@@ -142,6 +152,10 @@
             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:
             /*
@@ -203,10 +217,12 @@
             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();//Mise en cache de toute les instructions
+            
             gameEtat = ETAT_GAME_WAIT_FOR_JACK;
             SendRawId(ECRAN_ACK_START_MATCH);
             tactile_printf("Attente du JACK.");
@@ -230,10 +246,13 @@
             //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;
+            
+            if (ModeDemo == 0){
+                chronoEnd.attach(&chronometre_ISR,90);//On lance le chrono de 90s
+                gameTimer.start();
+            } 
             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
@@ -243,6 +262,7 @@
             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
@@ -297,6 +317,7 @@
                     }
                     localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1);
                     GoStraight(localData2, 0, 0, localData5);
+                    
                 break;
                 case MV_TURN: //Rotation sur place
                     if(instruction.direction == RELATIVE) {
@@ -358,11 +379,13 @@
                     GoStraight(localData2, localData5, localData3, 0);
                 break;
                 case ACTION:
+                    SendRawId(0x300);
                     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) {
+                            ///wait_us(200);
                             actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
                             gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
                         } else {
@@ -373,6 +396,7 @@
                         //C'est un AX12 qu'il faut bouger
                         AX12_setGoal(instruction.arg1,instruction.arg3/10,instruction.arg2);
                         AX12_enchainement++;
+                                                
                     }
                 break;
                 default:
@@ -401,7 +425,7 @@
             /*
             Attente de l'ack de l'instruction
             */
-            if(waitingAckID == 0 && waitingAckFrom ==0) {//Les ack ont été reset, c'est bon on continu
+            if(waitingAckID == 0 && waitingAckFrom == 0) {//Les ack ont été reset, c'est bon on continu
             //if(true) {
                 cartesCheker.stop();
                 if(instruction.nextActionType == JUMP) {
@@ -437,8 +461,16 @@
                             waitingAckFrom = INSTRUCTION_END_MOTEUR;
                         break;
                         case ACTION:
-                            waitingAckFrom = 0;//SERVO_AX12_DONE;
-                            waitingAckID = 0;//instruction.arg1;
+                            SendRawId(0x302);
+                            waitingAckFrom = 0; //SERVO_AX12_DONE;
+                            waitingAckID = 0;// instruction.arg1;
+                            /*if (modeTelemetre == 0){
+                                
+                            }else{ // si on attend la reponse du telemetre
+                                waitingAckFrom = 0;//TELEMETRE_RECHERCHE_OBJET;
+                                waitingAckID = OBJET_SUR_TABLE;   
+                                //modeTelemetre = 0;  
+                            }*/
                         break;
                         default:
                         break;
@@ -505,9 +537,13 @@
         
         
         
-        case ETAT_END:
-           
-            gameEtat = ETAT_END_LOOP;
+        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
@@ -555,6 +591,7 @@
             case ACKNOWLEDGE_BALISE:
             case ACKNOWLEDGE_MOTEUR:
             case ACKNOWLEDGE_IHM:
+            case ACKNOWLEDGE_TELEMETRE:
             case INSTRUCTION_END_BALISE:
             case INSTRUCTION_END_MOTEUR:
             case INSTRUCTION_END_IHM:
@@ -580,17 +617,25 @@
             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));
+                //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);
+                SendAck(0x114, SERVO_AX12_PROCESS);
+                //AX12_processChange(1);
             break;
+            
             case SERVO_AX12_DONE:
-                //SendAck(0x114, SERVO_AX12_DONE);
+                SendRawId(0x666);
+                /*//SendAck(0x114, SERVO_AX12_DONE);
                 AX12_notifyCANEnd(((unsigned short)(msgRxBuffer[FIFO_lecture].data[0])));
+                
+                /*gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
+                waitingAckFrom = 0;
+                waitingAckID = 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
@@ -609,6 +654,11 @@
                     
                 }
             break;
+                
+            case TELEMETRE_RECHERCHE_COIN:
+                SendRawId(RESET_TELEMETRE);
+            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
@@ -617,6 +667,14 @@
                     msgTx.type=CANData;
                     if(SelectStrategy(msgRxBuffer[FIFO_lecture].data[0])) {
                         // id de la stratégie sur 1 octet
+                        if (msgRxBuffer[FIFO_lecture].data[0] < 0x10){
+                                // Si la strat est une strat de match, on desactive le mode demo
+                                ModeDemo = 0;
+                        } else {
+                                // sinon, on active le mode demo, utile pour la fin de la demo
+                                ModeDemo = 1;
+                        }
+                        
                         msgTx.data[0]=msgRxBuffer[FIFO_lecture].data[0];
                     } else {
                         //erreur sur 1 octet
@@ -664,6 +722,13 @@
                     SendRawId(ECRAN_ACK_CHOICE_START_ACTION);
                 }
             break;
+            
+            case OBJET_SUR_TABLE:
+                if ((gameEtat == ETAT_GAME_WAIT_ACK)&&(modeTelemetre)){
+                    strat_instructions[actual_instruction+1].arg1 = msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8);
+                    strat_instructions[actual_instruction+1].arg2 = msgRxBuffer[FIFO_lecture].data[2]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[3])<<8);
+                }
+            break;
         }        
         
         FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;