Diff: Strategie/Strategie.cpp

Revision:

11:ed13a480ddca

Parent:

10:a788d9cf60f2

Child:

12:14729d584500

--- a/Strategie/Strategie.cpp	Fri Apr 29 09:14:27 2016 +0000
+++ b/Strategie/Strategie.cpp	Mon May 02 19:40:59 2016 +0000
@@ -19,12 +19,12 @@
 signed short x_robot,y_robot,theta_robot;//La position du robot
 
 #ifdef ROBOT_BIG
-unsigned short id_check[NOMBRE_CARTES]= {CHECK_BALISE,CHECK_MOTEUR};
-unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_BALISE,ALIVE_MOTEUR};
+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
 #else
-unsigned short id_check[NOMBRE_CARTES]= {CHECK_BALISE,CHECK_MOTEUR};
-unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_BALISE,ALIVE_MOTEUR};
+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;
@@ -87,7 +87,7 @@
     if(lastEtat != gameEtat || debugetatTimer.read_ms() >= 1000) {
         lastEtat = gameEtat;
         debugetatTimer.reset();
-        sendStratEtat((unsigned char)gameEtat);
+        sendStratEtat((unsigned char)gameEtat, (unsigned char)actual_instruction);
     }
     
     switch(gameEtat)
@@ -131,7 +131,7 @@
             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
+            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
@@ -147,6 +147,7 @@
                 cartesCheker.stop();
                 screenChecktry = 0;
                 countAliveCard++;
+                checkCurrent++;
                 if(checkCurrent >= NOMBRE_CARTES) {
                     if(countAliveCard >= NOMBRE_CARTES) {
                         gameEtat = ETAT_CONFIG;
@@ -157,13 +158,13 @@
                         waitingAckFrom = ECRAN_ALL_CHECK;
                     }
                 } else {
-                    checkCurrent++;
                     gameEtat = ETAT_CHECK_CARTES;
                 }
             } else if(cartesCheker.read_ms () > 100) {
                 cartesCheker.stop();
                 if(screenChecktry >=3) {
                     screenChecktry = 0;
+                    checkCurrent++;
                     if(checkCurrent >= NOMBRE_CARTES) {
                         if(countAliveCard == NOMBRE_CARTES) {
                             gameEtat = ETAT_CONFIG;
@@ -173,7 +174,6 @@
                             waitingAckFrom = ECRAN_ALL_CHECK;
                         }
                     } else {
-                        checkCurrent++;
                         gameEtat = ETAT_CHECK_CARTES;
                     }
                 } else {
@@ -204,12 +204,12 @@
             SendRawId(ECRAN_ACK_START_MATCH);
             tactile_printf("Attente 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
             SetOdometrie(ODOMETRIE_BIG_POSITION, POSITION_DEBUT_X,POSITION_DEBUT_Y,POSITION_DEBUT_T);
 #else
             SetOdometrie(ODOMETRIE_SMALL_POSITION, POSITION_DEBUT_X,POSITION_DEBUT_Y,POSITION_DEBUT_T);
-#endif
+#endif*/
         break;
         case ETAT_GAME_WAIT_FOR_JACK:
             //TODO Attendre le jack
@@ -260,8 +260,7 @@
                             localData5 = 0;
                         }
                     }
-                    
-                    localData1 = ((instruction.direction == FORWARD)?1:-1);
+                    localData1 = ((instruction.direction == LEFT)?1:-1);
                     BendRadius(instruction.arg1, instruction.arg3, localData1, localData5);
                 break;
                 case MV_LINE:
@@ -318,21 +317,40 @@
                 case MV_RECALAGE:
                     waitingAckID = ASSERVISSEMENT_RECALAGE;
                     waitingAckFrom = ACKNOWLEDGE_MOTEUR;
-                    //TODO - mettre l'ACK à jour
+                    instruction.nextActionType = WAIT;
+                    localData2 = (((instruction.direction == FORWARD)?1:-1)*3000);
+                    
+                    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++;
                     }
-                    waitingAckFrom = 0;
-                    waitingAckID = 0;
-                    actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
-                    gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
-                    return;
                 break;
                 default:
                     //Instruction inconnue, on l'ignore
@@ -364,14 +382,12 @@
             //if(true) {
                 cartesCheker.stop();
                 if(instruction.nextActionType == JUMP) {
-                    if(instruction.jumpAction == JUMP_TIME) {
+                    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.jumpAction == JUMP_POSITION) {
-                        gameEtat = ETAT_GAME_JUMP_POSITION;
-                    } else {
-                        //ERROR   
+                        cartesCheker.start();  
                     }
                 } else if(instruction.nextActionType == WAIT) {
                     gameEtat = ETAT_GAME_WAIT_END_INSTRUCTION;
@@ -494,6 +510,8 @@
             case ALIVE_BALISE:
             case ALIVE_MOTEUR:
             case ALIVE_IHM:
+            case ALIVE_ACTIONNEURS:
+            case ALIVE_POMPES:
             case ECRAN_ALL_CHECK:
                 if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id) {
                     waitingAckFrom = 0;//C'est la bonne carte qui indique qu'elle est en ligne
@@ -506,7 +524,7 @@
             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)) {
+                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;
                 }