Diff: Strategie/Strategie.cpp

Revision:

27:76ead555a63d

Parent:

26:2f4fcc2354f3

Child:

28:acd18776ed2d

--- a/Strategie/Strategie.cpp	Fri May 26 04:58:47 2017 +0000
+++ b/Strategie/Strategie.cpp	Fri May 26 19:01:56 2017 +0000
@@ -240,7 +240,9 @@
             localData2 = POSITION_DEBUT_T;
             localData3 = POSITION_DEBUT_Y;
             if(InversStrat == 1) {
-                localData2 = -localData2;//Inversion theta
+                localData2 = 1800-localData2;//Inversion theta
+                if(localData2 > 1800) localData2 -= 1800;
+                else if(localData2 <= -1800) localData2 += 1800;
                 localData3 = 3000 - POSITION_DEBUT_Y;//Inversion du Y
             }
             SetOdometrie(ODOMETRIE_BIG_POSITION, POSITION_DEBUT_X,localData3,localData2);
@@ -399,7 +401,9 @@
                     #ifdef ROBOT_BIG
                     if(InversStrat == 1) {
                          localData1 = -localData1;
-                         localData3 = -localData3;
+                         localData2 = 1800-instruction.arg3;//Inversion theta
+                        if(localData2 > 1800) localData2 -= 1800;
+                        else if(localData2 <= -1800) localData2 += 1800;
                     }
                     #endif
                     
@@ -653,16 +657,23 @@
             
             switch(actionPrecedente){
                 case MV_LINE:
+                
                     if(instruction.direction == BACKWARD) {
                         localData1 = -1;
                     } else {
                         localData1 = 1;
                     }
+                    
                     GoToPosition(target_x_robot,target_y_robot,target_theta_robot,localData1);
-                    break;
+                    debugXYTTarget(target_x_robot,target_y_robot,target_theta_robot);
+                    waitingAckID = ASSERVISSEMENT_XYT;
+                    waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+                    gameEtat = ETAT_GAME_WAIT_ACK; 
+                    instruction.order = MV_XYT;
+                    return;
                     
                 case MV_XYT:
-                    GoToPosition(target_x_robot,target_y_robot,target_theta_robot,localData1);
+                    gameEtat = ETAT_GAME_PROCESS_INSTRUCTION; 
                     break;
                     
                 case MV_COURBURE:
@@ -673,10 +684,14 @@
                         }
                     BendRadius(target_theta_robot, localData2, localData1, localData5);
                     break;
+                default:
+                    actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
+                    gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
+                break;
             }
             
-            actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
-            gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
+            //actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
+            //gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
         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;
@@ -831,7 +846,7 @@
                     }
                     can1.write(msgTx);
                     wait_ms(10);
-                    setAsservissementEtat(0);//Désactivation de l'asservissement pour repositionner le robot dans le zone de départ
+                    //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;
@@ -841,17 +856,17 @@
             
             case BALISE_STOP:
                 SendAck(ACKNOWLEDGE_BALISE, BALISE_STOP);
-                //if (instruction[actual_instruction].order != MV_TURN){ //J'ai rajouté cette ligne mais il faut tester avec et sans pour voir le comportement du robot, 
+                //if (instruction[actual_instruction].order != MV_TURN && instruction[actual_instruction].order != ACTION ){ //J'ai rajouté cette ligne mais il faut tester avec et sans pour voir le comportement du robot, 
                     if(needToStop() != 0 && ingnorBaliseOnce ==0) {
                         if(gameEtat > ETAT_GAME_START && gameEtat != ETAT_WARNING_TIMEOUT)
                         {
                             SendRawId(ASSERVISSEMENT_STOP);
                             //while(1); // ligne à décommenter si on est en homologation
-                            gameEtat = ETAT_WARNING_TIMEOUT;
                             if(gameEtat != ETAT_WARING_END_BALISE_WAIT) {
                                 timeoutWarning.reset();
                                 timeoutWarning.start();//Reset du timer utiliser par le timeout
                             }
+                            gameEtat = ETAT_WARNING_TIMEOUT;
                         }
                     }
                 //}