Diff: Strategie/Strategie.cpp

Revision:

76:b5eb99354389

Parent:

75:1db1b929f13d

Child:

77:641c7f1a827c

--- a/Strategie/Strategie.cpp	Mon May 27 12:38:21 2019 +0000
+++ b/Strategie/Strategie.cpp	Mon May 27 18:51:34 2019 +0000
@@ -107,7 +107,7 @@
 
 unsigned char ingnorBaliseOnce = 0;//une fois détecté réinitialise
 unsigned char ingnorBalise = 0;//0:balise ignore 1:on ecoute la balise
-unsigned char robot_arrete = 0;
+short direction;
 
 unsigned char ingnorInversionOnce = 0;//Pour ignorer l'inversion des instruction une fois
 
@@ -1140,8 +1140,19 @@
                         localData1 = -localData1;//Inversion de la direction
                     }
                     BendRadius(instruction.arg1, localData2, localData1, localData5);
-
-
+#ifdef ROBOT_SMALL
+                    if(localData2>0) {
+                        direction=1;
+                    } else {
+                        direction=-1;
+                    }
+#else
+                    if(localData2>0) {
+                        direction=-1;
+                    } else {
+                        direction=1;
+                    }
+#endif
                     target_theta_robot =  localData2 - theta_robot;
                     /*
                     if(instruction.direction == LEFT){
@@ -1174,24 +1185,7 @@
 
                     break;
                 case MV_TURN: //Rotation sur place
-                    /*if(instruction.direction == RELATIVE) {
-                        localData2 = instruction.arg3;
-                    } else {//C'est un rotation absolu, il faut la convertir en relative
-                        localData2 = instruction.arg3;
-
-                        localData2 = (localData2 - theta_robot)%3600;
-                        if(localData2 > 1800) {
-                            localData2 = localData2-3600;
-                        }
-                        else if(localData2 <-1800){
-                            localData2 = localData2+3600;
-                        }
-                    }
-                    if(InversStrat == 1 && ingnorInversionOnce == 0) {
-                        localData2 = -localData2;
-                    }*/
-
-
+            
                     localData2 = instruction.arg3;
 
                     if(InversStrat == 1 && ingnorInversionOnce == 0) {
@@ -1212,7 +1206,6 @@
                     waitingAckFrom = ACKNOWLEDGE_MOTEUR;
                     Rotate(localData2);
 
-
                     break;
                 case MV_XYT:
                     if(instruction.direction == BACKWARD) {
@@ -2056,36 +2049,38 @@
                     angle_moyen_balise_IR = debut_angle_detection + (fin_angle_detection-debut_angle_detection)/2;
 
 #ifdef ROBOT_BIG
-                /*   float seuil_bas_avant = 12.0;  // >=   
-                   float seuil_haut_avant = 0.0; // <=
-                   float seuil_bas_arriere = 4.0;
-                   float seuil_haut_arriere = 8.0;*/
-                float seuil_bas_avant = 13.0;
+                SendRawId(0x663);
+                float seuil_bas_avant = 12.0;  // >=
+                float seuil_haut_avant = 0.0; // <=
+                float seuil_bas_arriere = 4.0;
+                float seuil_haut_arriere = 8.0;
+                /*float seuil_bas_avant = 13.0;
                 float seuil_haut_avant = 15.0;
                 float seuil_bas_arriere = 5.0;
-                float seuil_haut_arriere = 7.0;
+                float seuil_haut_arriere = 7.0;*/
 #else
+                SendRawId(0x664);
                 float seuil_bas_avant = 13.0;
                 float seuil_haut_avant = 15.0;
                 float seuil_bas_arriere = 5.0;
                 float seuil_haut_arriere = 7.0;
 #endif
+                SendRawId(0x665);
                 if (instruction.order == MV_LINE && instruction.direction == FORWARD  && angle_moyen_balise_IR >= seuil_bas_avant && angle_moyen_balise_IR <= seuil_haut_avant
                         || instruction.order == MV_LINE && instruction.direction == BACKWARD && angle_moyen_balise_IR >= seuil_bas_arriere && angle_moyen_balise_IR <= seuil_haut_arriere
-                        || instruction.order == MV_COURBURE && instruction.direction == FORWARD && angle_moyen_balise_IR >= seuil_bas_avant && angle_moyen_balise_IR <= seuil_haut_avant
-                        || instruction.order == MV_COURBURE && instruction.direction == BACKWARD && angle_moyen_balise_IR >= seuil_bas_arriere  && angle_moyen_balise_IR <= seuil_haut_arriere
+                        || instruction.order == MV_COURBURE && direction == 1 && angle_moyen_balise_IR >= seuil_bas_avant && angle_moyen_balise_IR <= seuil_haut_avant
+                        || instruction.order == MV_COURBURE && direction == -1 && angle_moyen_balise_IR >= seuil_bas_arriere  && angle_moyen_balise_IR <= seuil_haut_arriere
                         || instruction.order == MV_XYT && instruction.direction == FORWARD && angle_moyen_balise_IR >= seuil_bas_avant  && angle_moyen_balise_IR <= seuil_haut_avant
                         || instruction.order == MV_XYT && instruction.direction == BACKWARD && angle_moyen_balise_IR >= seuil_bas_arriere  && angle_moyen_balise_IR <= seuil_haut_arriere ) { //J'ai rajouté cette ligne mais il faut tester avec et sans pour voir le comportement du robot,
-
+                    SendRawId(0x666);
                     if(needToStop() != 0 && ingnorBaliseOnce ==0 && ingnorBalise==0) {
+                        SendRawId(0x667);
                         if(gameEtat > ETAT_GAME_START && gameEtat != ETAT_WARNING_TIMEOUT) {
                             SendRawId(ASSERVISSEMENT_STOP);
-
                             /*waitingAckID_FIN = ASSERVISSEMENT_STOP;
                             waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
                             while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0)
                                 canProcessRx();*/
-
                             //while(1); // ligne à décommenter si on est en homologation
                             if(gameEtat != ETAT_WARING_END_BALISE_WAIT) {
                                 timeoutWarning.reset();
@@ -2095,6 +2090,7 @@
                         }
                     }
                 }
+                SendRawId(0x668);
                 ingnorBaliseOnce = 0;
                 break;