Diff: Strategie/Strategie.cpp

Revision:

25:f140c93a8666

Parent:

24:4a28a72781a6

Child:

26:2f4fcc2354f3

--- a/Strategie/Strategie.cpp	Thu May 25 17:17:42 2017 +0000
+++ b/Strategie/Strategie.cpp	Fri May 26 01:27:49 2017 +0000
@@ -18,7 +18,8 @@
 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
-
+signed short target_x_robot, target_y_robot, target_theta_robot;
+E_InstructionType actionPrecedente;
 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
@@ -289,6 +290,9 @@
             Traitement de l'instruction, envoie de la trame CAN
             */
             //debug_Instruction(instruction);
+            
+            actionPrecedente = instruction.order;
+            
             switch(instruction.order)
             {
                 case MV_COURBURE://C'est un rayon de courbure
@@ -311,11 +315,11 @@
                     {
                         localData1 = -localData1;//Inversion de la direction
                         #ifdef ROBOT_BIG
-                        localData2 = localData2;
+                        localData2 = -localData2;
                         #endif
                     }
+                    BendRadius(instruction.arg1, localData2, localData1, localData5);
                     
-                    BendRadius(instruction.arg1, localData2, localData1, localData5);
                 break;
                 case MV_LINE://Ligne droite
                     waitingAckID = ASSERVISSEMENT_RECALAGE;
@@ -341,6 +345,10 @@
                     localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1);
                     GoStraight(localData2, 0, 0, localData5);
                     
+                    target_x_robot = x_robot + localData2*cos(theta_robot); 
+                    target_y_robot = y_robot + localData2*sin(theta_robot);
+                    target_theta_robot = theta_robot;
+                    
                 break;
                 case MV_TURN: //Rotation sur place
                     if(instruction.direction == RELATIVE) {
@@ -388,6 +396,11 @@
                     GoToPosition(instruction.arg1,localData3,localData2,localData1);
                     waitingAckID = ASSERVISSEMENT_XYT;
                     waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+                    
+                    target_x_robot = instruction.arg1; 
+                    target_y_robot = localData3;
+                    target_theta_robot = localData2;
+                    
                 break;
                 case MV_RECALAGE:
                     waitingAckID = ASSERVISSEMENT_RECALAGE;
@@ -618,6 +631,7 @@
             }
         break;
         case ETAT_WARNING_END_LAST_INSTRUCTION://trouver le meilleur moyen de reprendre l'instruction en cours
+/*
 #ifdef ROBOT_BIG
             actual_instruction = instruction.nextLineError;//  2 //Modification directe... c'est pas bien mais ça marchait pour le match 5
             gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
@@ -625,8 +639,30 @@
             actual_instruction = instruction.nextLineError;
             gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; 
 #endif      
-            gameEtat = ETAT_END;
+            gameEtat = ETAT_END;*/
             
+            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;
+                    
+                case MV_XYT:
+                    
+                    GoToPosition(target_x_robot,target_y_robot,target_theta_robot,localData1);
+                    break;
+                    
+                case MV_TURN:
+                    while(1);
+                    break;
+            }
+            
+            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;