Diff: Strategie/Strategie.cpp

Revision:

12:14729d584500

Parent:

11:ed13a480ddca

Child:

14:c8fc06c4887f

--- a/Strategie/Strategie.cpp	Mon May 02 19:40:59 2016 +0000
+++ b/Strategie/Strategie.cpp	Mon May 09 09:10:17 2016 +0000
@@ -18,10 +18,12 @@
 
 signed short x_robot,y_robot,theta_robot;//La position du robot
 
+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};
-unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_BALISE,ALIVE_MOTEUR,ALIVE_ACTIONNEURS};
-InterruptIn jack(p24); //  entrée analogique en interruption pour le jack
+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};
+InterruptIn jack(p25); //  entrée analogique en interruption pour le jack
 #else
 unsigned short id_check[NOMBRE_CARTES]= {CHECK_BALISE,CHECK_MOTEUR,CHECK_ACTIONNEURS};
 unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_BALISE,ALIVE_MOTEUR,ALIVE_ACTIONNEURS};
@@ -34,6 +36,8 @@
 
 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                                */
@@ -45,6 +49,7 @@
     gameTimer.stop();//Arret du timer
 
 #ifdef ROBOT_BIG
+    wait_ms(2000);
     doFunnyAction();
 #endif
     
@@ -78,7 +83,7 @@
     signed short localData4 = 0;
     unsigned char localData5 = 0;
     
-    if(gameTimer.read_ms() >= 88000) {//Fin du match (On autorise 2s pour déposer des éléments
+    if(gameTimer.read_ms() >= 89000) {//Fin du match (On autorise 2s pour déposer des éléments
         gameTimer.stop();
         gameTimer.reset();
         gameEtat = ETAT_END;//Fin du temps
@@ -149,6 +154,7 @@
                 countAliveCard++;
                 checkCurrent++;
                 if(checkCurrent >= NOMBRE_CARTES) {
+                    //printf("all card check, missing %d cards\n",(NOMBRE_CARTES-countAliveCard));
                     if(countAliveCard >= NOMBRE_CARTES) {
                         gameEtat = ETAT_CONFIG;
                         SendRawId(ECRAN_ALL_CHECK);//On dit à l'IHM que toutes les cartes sont en ligne
@@ -163,8 +169,10 @@
             } else if(cartesCheker.read_ms () > 100) {
                 cartesCheker.stop();
                 if(screenChecktry >=3) {
+                    //printf("missing card %d\n",id_check[checkCurrent]);
                     screenChecktry = 0;
                     checkCurrent++;
+                    
                     if(checkCurrent >= NOMBRE_CARTES) {
                         if(countAliveCard == NOMBRE_CARTES) {
                             gameEtat = ETAT_CONFIG;
@@ -198,29 +206,37 @@
         break;
         case ETAT_GAME_INIT:
             //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
             gameEtat = ETAT_GAME_WAIT_FOR_JACK;
             SendRawId(ECRAN_ACK_START_MATCH);
             tactile_printf("Attente du JACK.");
+            setAsservissementEtat(1);//On réactive l'asservissement
+            jack.mode(PullDown); // désactivation de la résistance interne 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
+         
+#ifdef ROBOT_BIG //le gros robot n'a pas de recalage bordure pour ce placer au début, on lui envoit donc ça position
+            localData2 = POSITION_DEBUT_T;
+            localData3 = POSITION_DEBUT_Y;
+            if(InversStrat == 1) {
+                localData2 = -localData2;//Inversion theta
+                localData3 = 3000 - POSITION_DEBUT_Y;//Inversion du Y
+            }
+            SetOdometrie(ODOMETRIE_BIG_POSITION, POSITION_DEBUT_X,localData3,localData2);
+#endif /*
             SetOdometrie(ODOMETRIE_SMALL_POSITION, POSITION_DEBUT_X,POSITION_DEBUT_Y,POSITION_DEBUT_T);
 #endif*/
         break;
         case ETAT_GAME_WAIT_FOR_JACK:
-            //TODO Attendre le jack
+            //On attend le jack
         break;
         case ETAT_GAME_START:
-            chronoEnd.attach(&chronometre_ISR,90);
+            chronoEnd.attach(&chronometre_ISR,90);//On lance le chrono de 90s
             gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
             gameTimer.reset();
             gameTimer.start();
-            jack.fall(NULL);
+            jack.fall(NULL);//On désactive l'interruption du jack
             SendRawId(GLOBAL_START);
+            tactile_printf("Start");//Pas vraiment util mais bon
         break;
         case ETAT_GAME_LOAD_NEXT_INSTRUCTION:
             /*
@@ -234,10 +250,8 @@
                 instruction = strat_instructions[actual_instruction];
                 //On effectue le traitement de l'instruction 
                 gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;
-                //actual_instruction++;
             }
             screenChecktry = 0;
-            wait_ms(100);
         break;
         case ETAT_GAME_PROCESS_INSTRUCTION:
             /*
@@ -246,7 +260,7 @@
             //debug_Instruction(instruction);
             switch(instruction.order)
             {
-                case MV_COURBURE:
+                case MV_COURBURE://C'est un rayon de courbure
                     waitingAckID = ASSERVISSEMENT_COURBURE;
                     waitingAckFrom = ACKNOWLEDGE_MOTEUR;
                     if(instruction.nextActionType == ENCHAINEMENT) {
@@ -261,16 +275,20 @@
                         }
                     }
                     localData1 = ((instruction.direction == LEFT)?1:-1);
+                    if(InversStrat == 1)
+                    {
+                        localData1 = -localData1;//Inversion de la direction
+                    }
                     BendRadius(instruction.arg1, instruction.arg3, localData1, localData5);
                 break;
-                case MV_LINE:
+                case MV_LINE://Ligne droite
                     waitingAckID = ASSERVISSEMENT_RECALAGE;
                     waitingAckFrom = ACKNOWLEDGE_MOTEUR;
                     if(instruction.nextActionType == ENCHAINEMENT) {
                         MV_enchainement++;
                         localData5 = 1;
                     } else {
-                        if(MV_enchainement > 0) {
+                        if(MV_enchainement > 0) {//Utilisé en cas d'enchainement, 
                             localData5 = 2;
                             MV_enchainement = 0;
                         } else {
@@ -280,19 +298,23 @@
                     localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1);
                     GoStraight(localData2, 0, 0, localData5);
                 break;
-                case MV_TURN:
+                case MV_TURN: //Rotation sur place
                     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;
+                    } else {//C'est un rotation absolu, il faut la convertir en relative
+                        localData2 = instruction.arg3;
+                        
+                        if(InversStrat == 1) {
+                            localData2 = -localData2;
                         }
+                        
+                        localData2 = (localData2 - theta_robot)%3600;
+                        if(localData2 > 1800) {
+                            localData2 = localData2-3600;
+                        }
+                        
                     }
-                    if(InversStrat == 1) {
-                        localData2 = -localData2;
-                    }
+                    
                     Rotate(localData2);
                     waitingAckID = ASSERVISSEMENT_ROTATION;
                     waitingAckFrom = ACKNOWLEDGE_MOTEUR;
@@ -305,12 +327,13 @@
                     }
                     
                     if(InversStrat == 1) {
-                        //localData2 = 360 - instruction.arg3
+                        localData2 = -instruction.arg3;
                         localData3 = 3000 - instruction.arg2;//Inversion du Y
                     } else {
                         localData3 = instruction.arg2;
+                        localData2 = instruction.arg3;
                     }
-                    GoToPosition(instruction.arg1,localData3,instruction.arg3,localData1);
+                    GoToPosition(instruction.arg1,localData3,localData2,localData1);
                     waitingAckID = ASSERVISSEMENT_XYT;
                     waitingAckFrom = ACKNOWLEDGE_MOTEUR;
                 break;
@@ -318,7 +341,7 @@
                     waitingAckID = ASSERVISSEMENT_RECALAGE;
                     waitingAckFrom = ACKNOWLEDGE_MOTEUR;
                     instruction.nextActionType = WAIT;
-                    localData2 = (((instruction.direction == FORWARD)?1:-1)*3000);
+                    localData2 = (((instruction.direction == FORWARD)?1:-1)*3000);//On indique une distance de 3000 pour etre sur que le robot va ce recaler
                     
                     if(instruction.precision == RECALAGE_Y) {
                         localData5 = 2;
@@ -454,20 +477,30 @@
         
         
         case ETAT_WARNING_TIMEOUT://Attente de la trame fin de danger ou du timeout de 2s
-            if(timeoutWarning.read_ms() >= 2000)//ça fait plus de 2s, il faut changer de stratégie
+            if(timeoutWarning.read_ms() >= BALISE_TIMEOUT)//ça fait plus de 2s, il faut changer de stratégie
             {
                 gameEtat = ETAT_WARNING_SWITCH_STRATEGIE;
             }
         break;
         case ETAT_WARING_END_BALISE_WAIT://Attente d'une seconde apres la fin d'un End Balise pour etre sur que c'est bon
-            
+            if(timeoutWarningWaitEnd.read_ms() >= 1000) {//c'est bon, on repart
+                //actual_instruction = instruction.nextLineError;
+                gameEtat = ETAT_WARNING_END_LAST_INSTRUCTION;
+            }
         break;
         case ETAT_WARNING_END_LAST_INSTRUCTION://trouver le meilleur moyen de reprendre l'instruction en cours
-            
+#ifdef ROBOT_BIG
+            actual_instruction = 2;//Modification directe... c'est pas bien mais ça marchait pour le match 5
+            gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
+#else
+            actual_instruction = instruction.nextLineError;
+            gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; 
+#endif
         break;
         case ETAT_WARNING_SWITCH_STRATEGIE://Si à la fin du timeout il y a toujours un robot, passer à l'instruction d'erreur
             actual_instruction = instruction.nextLineError;
             gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
+            ingnorBaliseOnce = 1;
         break;
         
         
@@ -512,6 +545,7 @@
             case ALIVE_IHM:
             case ALIVE_ACTIONNEURS:
             case ALIVE_POMPES:
+            case ALIVE_AX12:
             case ECRAN_ALL_CHECK:
                 if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id) {
                     waitingAckFrom = 0;//C'est la bonne carte qui indique qu'elle est en ligne
@@ -590,19 +624,26 @@
                     }
                     can1.write(msgTx);
                     wait_ms(10);
-                    tactile_printf("Strat %d selectionne",msgTx.data[0]);
+                    setAsservissementEtat(0);//Désactivation de l'asservissement pour repositionner le robot dans le zone de départ
+                    tactile_printf("Strat %d, Asser desactive",msgTx.data[0]);
                 }
             break;
             case BALISE_STOP:
                 SendAck(ACKNOWLEDGE_BALISE, BALISE_STOP);
-                if(gameEtat > ETAT_GAME_START && gameEtat != ETAT_WARNING_TIMEOUT)
-                {
-                    wait_ms(10);
-                    SendRawId(ASSERVISSEMENT_STOP);
-                    gameEtat = ETAT_WARNING_TIMEOUT;
-                    timeoutWarning.reset();
-                    timeoutWarning.start();//Reset du timer utiliser par le timeout
+                
+                if(needToStop() != 0 && ingnorBaliseOnce ==0) {
+                    if(gameEtat > ETAT_GAME_START && gameEtat != ETAT_WARNING_TIMEOUT)
+                    {
+                        SendRawId(ASSERVISSEMENT_STOP);
+                        //while(1);
+                        gameEtat = ETAT_WARNING_TIMEOUT;
+                        if(gameEtat != ETAT_WARING_END_BALISE_WAIT) {
+                            timeoutWarning.reset();
+                            timeoutWarning.start();//Reset du timer utiliser par le timeout
+                        }
+                    }
                 }
+                ingnorBaliseOnce = 0;
             break;
             case BALISE_END_DANGER:
                 if(gameEtat == ETAT_WARNING_TIMEOUT) {
@@ -611,6 +652,18 @@
                     gameEtat = ETAT_WARING_END_BALISE_WAIT;
                 }
             break;
+            
+            case ECRAN_CHOICE_START_ACTION:
+                if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config
+                    if(msgRxBuffer[FIFO_lecture].data[0] == 1) {
+                        runRobotTest();
+                    } else {
+                        initRobotActionneur();
+                    }
+                    wait_ms(500);
+                    SendRawId(ECRAN_ACK_CHOICE_START_ACTION);
+                }
+            break;
         }        
         
         FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;