Diff: Strategie/Strategie.cpp

Revision:

26:2f4fcc2354f3

Parent:

25:f140c93a8666

Child:

27:76ead555a63d

--- a/Strategie/Strategie.cpp	Fri May 26 01:27:49 2017 +0000
+++ b/Strategie/Strategie.cpp	Fri May 26 04:58:47 2017 +0000
@@ -1,4 +1,5 @@
  #include "Strategie.h"
+ #define M_PI           3.14159265358979323846
 
 E_stratGameEtat     gameEtat  = ETAT_CHECK_CARTE_SCREEN;
 E_stratGameEtat     lastEtat  = ETAT_CHECK_CARTE_SCREEN;
@@ -296,6 +297,7 @@
             switch(instruction.order)
             {
                 case MV_COURBURE://C'est un rayon de courbure
+                    actionPrecedente = MV_COURBURE;
                     waitingAckID = ASSERVISSEMENT_COURBURE;
                     waitingAckFrom = ACKNOWLEDGE_MOTEUR;
                     if(instruction.nextActionType == ENCHAINEMENT) {
@@ -316,10 +318,17 @@
                         localData1 = -localData1;//Inversion de la direction
                         #ifdef ROBOT_BIG
                         localData2 = -localData2;
+                        localData1 = -localData1;//Inversion de la direction
                         #endif
                     }
                     BendRadius(instruction.arg1, localData2, localData1, localData5);
                     
+                    if(instruction.direction == LEFT){
+                        target_theta_robot = theta_robot + localData2;
+                    }else{
+                        target_theta_robot = theta_robot - localData2;
+                        }
+                    
                 break;
                 case MV_LINE://Ligne droite
                     waitingAckID = ASSERVISSEMENT_RECALAGE;
@@ -345,8 +354,8 @@
                     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_x_robot = x_robot + localData2*cos((double)theta_robot*M_PI/1800); 
+                    target_y_robot = y_robot + localData2*sin((double)theta_robot*M_PI/1800);
                     target_theta_robot = theta_robot;
                     
                 break;
@@ -370,6 +379,7 @@
                     Rotate(localData2);
                     waitingAckID = ASSERVISSEMENT_ROTATION;
                     waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+                    
                 break;
                 case MV_XYT:
                     if(instruction.direction == BACKWARD) {
@@ -652,12 +662,16 @@
                     break;
                     
                 case MV_XYT:
-                    
                     GoToPosition(target_x_robot,target_y_robot,target_theta_robot,localData1);
                     break;
                     
-                case MV_TURN:
-                    while(1);
+                case MV_COURBURE:
+                    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);
                     break;
             }
             
@@ -832,7 +846,7 @@
                         if(gameEtat > ETAT_GAME_START && gameEtat != ETAT_WARNING_TIMEOUT)
                         {
                             SendRawId(ASSERVISSEMENT_STOP);
-                            while(1); // ligne à décommenter si on est en homologation
+                            //while(1); // ligne à décommenter si on est en homologation
                             gameEtat = ETAT_WARNING_TIMEOUT;
                             if(gameEtat != ETAT_WARING_END_BALISE_WAIT) {
                                 timeoutWarning.reset();