Diff: Strategie/Strategie.cpp

Revision:

28:acd18776ed2d

Parent:

27:76ead555a63d

Child:

29:41e02746041d

--- a/Strategie/Strategie.cpp	Fri May 26 19:01:56 2017 +0000
+++ b/Strategie/Strategie.cpp	Sat May 27 05:40:26 2017 +0000
@@ -51,6 +51,10 @@
 
 unsigned char ingnorBaliseOnce = 0;
 
+unsigned char ingnorInversionOnce = 0;//Pour ignorer l'inversion des instruction une fois
+
+struct S_Instruction instruction;
+
 
 /****************************************************************************************/
 /* FUNCTION NAME: chronometre_ISR                                                       */
@@ -88,7 +92,6 @@
 /****************************************************************************************/
 void automate_process(void)
 {
-    static struct S_Instruction instruction;
     static unsigned char AX12_enchainement = 0;
     static unsigned char MV_enchainement = 0;
     signed char localData1 = 0;
@@ -241,8 +244,8 @@
             localData3 = POSITION_DEBUT_Y;
             if(InversStrat == 1) {
                 localData2 = 1800-localData2;//Inversion theta
-                if(localData2 > 1800) localData2 -= 1800;
-                else if(localData2 <= -1800) localData2 += 1800;
+                if(localData2 > 1800) localData2 -= 3600;
+                else if(localData2 <= -1800) localData2 += 3600;
                 localData3 = 3000 - POSITION_DEBUT_Y;//Inversion du Y
             }
             SetOdometrie(ODOMETRIE_BIG_POSITION, POSITION_DEBUT_X,localData3,localData2);
@@ -287,6 +290,7 @@
                 gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;
             }
             screenChecktry = 0;
+            ingnorInversionOnce = 0;
         break;
         case ETAT_GAME_PROCESS_INSTRUCTION:
             /*
@@ -315,7 +319,7 @@
                     }
                     localData1 = ((instruction.direction == LEFT)?1:-1);
                     localData2 =  instruction.arg3;
-                    if(InversStrat == 1)
+                    if(InversStrat == 1 && ingnorInversionOnce == 0)
                     {
                         localData1 = -localData1;//Inversion de la direction
                         #ifdef ROBOT_BIG
@@ -325,11 +329,14 @@
                     }
                     BendRadius(instruction.arg1, localData2, localData1, localData5);
                     
+                    
+                    target_theta_robot =  localData2 - theta_robot;
+                    /*
                     if(instruction.direction == LEFT){
-                        target_theta_robot = theta_robot + localData2;
+                        
                     }else{
-                        target_theta_robot = theta_robot - localData2;
-                        }
+                        target_theta_robot = theta_robot + localData2;
+                        }*/
                     
                 break;
                 case MV_LINE://Ligne droite
@@ -347,7 +354,7 @@
                         }
                     }
                     #ifdef ROBOT_BIG
-                    if(InversStrat == 1) {
+                    if(InversStrat == 1 && ingnorInversionOnce == 0) {
                             /*if (instruction.direction == FORWARD) instruction.direction = BACKWARD;
                             else instruction.direction = FORWARD;*/
                             instruction.direction = ((instruction.direction == FORWARD)?BACKWARD:FORWARD);
@@ -374,14 +381,32 @@
                         
                     }
                     #ifdef ROBOT_SMALL
-                    if(InversStrat == 1) {
+                    if(InversStrat == 1 && ingnorInversionOnce == 0) {
                             localData2 = -localData2;
                         }
                     #endif
+                    #ifdef ROBOT_BIG
+                    if(InversStrat == 1 && ingnorInversionOnce == 0) {
+                        if(instruction.direction == RELATIVE) {
+                            localData2 = instruction.arg3;
+                        } else {//C'est un rotation absolu, il faut la convertir en relative
+                            
+                            localData2 = 1800-instruction.arg3;//Inversion theta
+                            if(localData2 > 1800) localData2 -= 3600;
+                            else if(localData2 <= -1800) localData2 += 3600;
+                            
+                            localData2 = (localData2 - theta_robot)%3600;
+                            if(localData2 > 1800) {
+                                localData2 = localData2-3600;
+                            }
+                            
+                        }
+                    }
+                    #endif  
                     Rotate(localData2);
                     waitingAckID = ASSERVISSEMENT_ROTATION;
                     waitingAckFrom = ACKNOWLEDGE_MOTEUR;
-                    
+                      
                 break;
                 case MV_XYT:
                     if(instruction.direction == BACKWARD) {
@@ -390,7 +415,7 @@
                         localData1 = 1;
                     }
                     
-                    if(InversStrat == 1) {
+                    if(InversStrat == 1 && ingnorInversionOnce == 0) {
                         localData2 = -instruction.arg3;
                         localData3 = 3000 - instruction.arg2;//Inversion du Y
                     } else {
@@ -399,11 +424,11 @@
                     }
                     
                     #ifdef ROBOT_BIG
-                    if(InversStrat == 1) {
+                    if(InversStrat == 1 && ingnorInversionOnce == 0) {
                          localData1 = -localData1;
                          localData2 = 1800-instruction.arg3;//Inversion theta
-                        if(localData2 > 1800) localData2 -= 1800;
-                        else if(localData2 <= -1800) localData2 += 1800;
+                         if(localData2 > 1800) localData2 -= 3600;
+                         else if(localData2 <= -1800) localData2 += 3600;
                     }
                     #endif
                     
@@ -424,7 +449,7 @@
                     
                     if(instruction.precision == RECALAGE_Y) {
                         localData5 = 2;
-                        if(InversStrat == 1) {
+                        if(InversStrat == 1 && ingnorInversionOnce == 0) {
                             localData3 = 3000 - instruction.arg1;//Inversion du Y
                         } else {
                             localData3 = instruction.arg1;
@@ -670,6 +695,11 @@
                     waitingAckFrom = ACKNOWLEDGE_MOTEUR;
                     gameEtat = ETAT_GAME_WAIT_ACK; 
                     instruction.order = MV_XYT;
+                    instruction.arg1 = target_x_robot;
+                    instruction.arg2 = target_y_robot;
+                    instruction.arg3 = target_theta_robot;
+                    instruction.direction = (localData1)?FORWARD:BACKWARD;
+                    ingnorInversionOnce = 1;//Pour éviter que l'ago recalcul l'inversion
                     return;
                     
                 case MV_XYT:
@@ -677,12 +707,27 @@
                     break;
                     
                 case MV_COURBURE:
+                    //target_theta_robot = theta_robot - target_theta_robot;
+                    //instruction.arg3 =  instruction.arg3 - target_theta_robot;
                     if(instruction.direction == LEFT){
                         target_theta_robot = target_theta_robot - theta_robot;
                     }else{
-                        target_theta_robot = target_theta_robot + theta_robot;
-                        }
-                    BendRadius(target_theta_robot, localData2, localData1, localData5);
+                        target_theta_robot = theta_robot + target_theta_robot;
+                    }
+                    
+                    
+                    target_theta_robot = (target_theta_robot)%3600;
+                    if(target_theta_robot > 1800) {
+                        target_theta_robot = target_theta_robot-3600;
+                    }
+                     if(InversStrat == 1) {
+                        target_theta_robot = -target_theta_robot;
+                    }
+                    instruction.arg3 = target_theta_robot;
+                    
+                    
+                    gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;                     
+                    
                     break;
                 default:
                     actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
@@ -856,7 +901,7 @@
             
             case BALISE_STOP:
                 SendAck(ACKNOWLEDGE_BALISE, BALISE_STOP);
-                //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 (instruction.order != MV_TURN && 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)
                         {
@@ -869,7 +914,7 @@
                             gameEtat = ETAT_WARNING_TIMEOUT;
                         }
                     }
-                //}
+                }
                 ingnorBaliseOnce = 0;
             break;