code avec modifs, programme mit dans les robots pour les derniers matchs

Dependencies:   mbed SerialHalfDuplex SDFileSystem liaison_Bluetooth ident_crac DISCO-F469NI_portrait

Files at this revision

API Documentation at this revision

Comitter:
goldmas
Date:
Sat Jul 17 11:07:17 2021 +0000
Parent:
36:c37dbe2be916
Commit message:
Dernier code de la coupe de fracne 2021

Changed in this revision

Arriver_bon_port/bon_port.cpp Show annotated file Show diff for this revision Revisions of this file
Arriver_bon_port/bon_port.h Show annotated file Show diff for this revision Revisions of this file
Asservissement/Asservissement.cpp Show annotated file Show diff for this revision Revisions of this file
Compteur_points/Compteur.cpp Show annotated file Show diff for this revision Revisions of this file
Compteur_points/Compteur.h Show annotated file Show diff for this revision Revisions of this file
Evitement/Evitement.cpp Show annotated file Show diff for this revision Revisions of this file
Evitement/Evitement.h Show annotated file Show diff for this revision Revisions of this file
Globals/global.h Show annotated file Show diff for this revision Revisions of this file
Globals/ident_crac.lib Show annotated file Show diff for this revision Revisions of this file
IHM/ihm.cpp Show annotated file Show diff for this revision Revisions of this file
Strategie/Strategie.cpp Show annotated file Show diff for this revision Revisions of this file
Strategie/Strategie.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Arriver_bon_port/bon_port.cpp	Sat Jul 17 11:07:17 2021 +0000
@@ -0,0 +1,137 @@
+#include "global.h"
+
+// {X,Y,T,Sens}
+int Nord_haut[4]={175,500,-90,1};
+int Sud_haut[4]={1500,500,-90,1};
+
+int Nord_bas[4]={175,500,-90,1};
+int Sud_bas[4]={1500,500,-90,1};
+
+int X_rob;
+int Y_rob;
+int T_rob;
+int Sens;
+
+int tourner;
+
+
+void arriver_bon_port(void)
+{   
+}
+/*
+//Variable de comptage des points
+port_lu = val_girou + 1;
+    
+////////////////////////////////Girouette Sud////////////////////////////////
+    if(val_girou == 0) //Sud
+    {
+        if(Hauteur == 0) //Bas
+        {
+            if(InversStrat == 1) 
+            {
+                X_rob = Sud_bas[0];
+                Y_rob = 3000 - Sud_bas[1];//Inversion du Y
+                T_rob = -Sud_bas[2];
+                Sens  = Sud_bas[3];
+            } 
+            else 
+            {
+                X_rob = Sud_bas[0];
+                Y_rob = Sud_bas[1];
+                T_rob = Sud_bas[2];
+                Sens  = Sud_bas[3];
+            }
+        }
+        else if(Hauteur == 1) //Haut
+        {
+            if(InversStrat == 1) 
+            {
+                X_rob = Sud_haut[0];
+                Y_rob = 3000 - Sud_haut[1];//Inversion du Y
+                T_rob = -Sud_haut[2];
+                Sens  = Sud_haut[3];
+            } 
+            else 
+            {
+                X_rob = Sud_haut[0];
+                Y_rob = Sud_haut[1];
+                T_rob = Sud_haut[2];
+                Sens  = Sud_haut[3];
+            }
+        }
+        
+        GoToPosition(X_rob, Y_rob, T_rob, Sens);
+        waitingAckID = ASSERVISSEMENT_XYT;
+        waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+        
+        //target_x_robot = X_rob;
+        //target_y_robot = Y_rob;
+        //target_theta_robot = T_rob;
+            
+        //GoStraight(Sens*350,0,0,0); 
+    }
+    
+////////////////////////////////Girouette Nord////////////////////////////////
+    else if(val_girou == 1) //Nord
+    {
+        if(Hauteur == 0) //Bas
+        {
+            if(InversStrat == 1) 
+            {
+                X_rob = Nord_bas[0];
+                Y_rob = 3000 - Nord_bas[1];//Inversion du Y
+                T_rob = -Nord_bas[2];
+                Sens  = Nord_bas[3];
+            } 
+            else 
+            {
+                X_rob = Nord_bas[0];
+                Y_rob = Nord_bas[1];
+                T_rob = Nord_bas[2];
+                Sens  = Nord_bas[3];
+            }   
+        }
+        else if(Hauteur == 1) //Haut
+        {
+            if(InversStrat == 1) 
+            {
+                X_rob = Nord_haut[0];
+                Y_rob = 3000 - Nord_haut[1];//Inversion du Y
+                T_rob = -Nord_haut[2];
+                Sens  = Nord_haut[3];
+            } 
+            else 
+            {
+                X_rob = Nord_haut[0];
+                Y_rob = Nord_haut[1];
+                T_rob = Nord_haut[2];
+                Sens  = Nord_haut[3];
+            } 
+        }
+        GoToPosition(X_rob, Y_rob, T_rob, Sens);
+        waitingAckID = ASSERVISSEMENT_XYT;
+        waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+    
+        //target_x_robot = X_rob;
+        //target_y_robot = Y_rob;
+        //target_theta_robot = T_rob;
+            
+        //GoStraight(Sens*350,0,0,0);   
+    }
+    
+////////////////////////////////Girouette Inconnu////////////////////////////////
+    else if(val_girou == 2) //Jsp
+    {
+        tourner = 3600;
+
+        if(InversStrat == 1) 
+        {
+            tourner = -tourner;
+        }
+
+        waitingAckID = ASSERVISSEMENT_ROTATION;
+        waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+        Rotate(tourner); 
+    }
+}
+*/ 
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Arriver_bon_port/bon_port.h	Sat Jul 17 11:07:17 2021 +0000
@@ -0,0 +1,6 @@
+#ifndef CRAC_BON_PORT
+#define CRAC_BON_PORT
+
+void arriver_bon_port(void);
+
+#endif
\ No newline at end of file
--- a/Asservissement/Asservissement.cpp	Mon May 31 13:36:03 2021 +0000
+++ b/Asservissement/Asservissement.cpp	Sat Jul 17 11:07:17 2021 +0000
@@ -22,7 +22,7 @@
     CANMessage msgTx=CANMessage();
     msgTx.id=id;
     msgTx.len=2;
-    msgTx.format=CANStandard;
+//    msgTx.format=CANStandard;
     msgTx.type=CANData;
     // from sur 2 octets
     msgTx.data[0]=(unsigned char)from;
@@ -37,7 +37,7 @@
     CANMessage msgTx=CANMessage();
     msgTx.id=id;
     msgTx.len=4;
-    msgTx.format=CANStandard;
+//    msgTx.format=CANStandard;
     msgTx.type=CANData;
     // from sur 2 octets
     msgTx.data[0]=(unsigned char)d1;
@@ -53,7 +53,7 @@
     CANMessage msgTx=CANMessage();
     msgTx.id=id;
     msgTx.len=len;
-    msgTx.format=CANStandard;
+//    msgTx.format=CANStandard;
     msgTx.type=CANData;
     // from sur 2 octets
     for(int i = 0; i<len; i++)
@@ -68,7 +68,7 @@
     CANMessage msgTx=CANMessage();
     msgTx.id=id;
     msgTx.len=1;
-    msgTx.format=CANStandard;
+//    msgTx.format=CANStandard;
     msgTx.type=CANData;
     msgTx.data[0]=data;
     
--- a/Compteur_points/Compteur.cpp	Mon May 31 13:36:03 2021 +0000
+++ b/Compteur_points/Compteur.cpp	Sat Jul 17 11:07:17 2021 +0000
@@ -1,24 +1,31 @@
 #include "global.h"
 
-#define M_PI 3.14159265358979323846
-#define DEMI_GOBELET 30
+#define M_PI 3.14159265358979323846f
+#define MARGE_VENT 72.0f
+#define MARGE_BRAS 54.0f
+#define ECART 100.0f
 
-int Ventouse_coord[6][2]={{145,-75},{145,0},{145,75},{-145,75},{-145,0},{-145,-75}};
-int Bras_coord[6][2];
-int Manche_air_coord[6][2];
+float Ventouse_coord[6][2]={{150,-80},{150,0},{150,80},{-150,80},{-150,0},{-150,-80}};
+float Bras_coord[6][2]={{232,-75},{232,0},{232,75},{-232,75},{-232,0},{-232,-75}};
+
+float Position_de_pose[6][2];
 
 int score_final=0;
 int score_ventouse=0,gobelet_vert=0,gobelet_rouge=0,gobelet_port=0;
 int score_manche=0,manche_releve=0;
-int score_bon_port=0;
-int score_phare=0;
-int score_pavillon=0;
+int score_bon_port=0, port_lu = 0;
+int score_phare=0, phare_active=0;
+int score_pavillon=0, pavillon=0;
+int offset_score=0;
 
-int etat_groupe, gobelet_en_place=0, old_gobelet=0;
+int etat_groupe[6], gobelet_en_place=0, old_gobelet=0;
 unsigned char num_groupe;
 
 CompteurGameEtat VentEtat[6] = {ETAT_ATTENDRE, ETAT_ATTENDRE, ETAT_ATTENDRE, ETAT_ATTENDRE, ETAT_ATTENDRE, ETAT_ATTENDRE};
-int deja_compter[6] = {0, 0, 0, 0, 0, 0};
+
+int old_gobelet_vert=0;
+int old_gobelet_rouge=0;
+int old_gobelet_port=0;
 
 /********************************************************************************************************************/
 /* FUNCTION NAME: gestion_Message_CAN                                                                               */
@@ -28,133 +35,255 @@
 {
     int identifiant = msgRxBuffer[FIFO_ecriture].id;
     
-    switch(identifiant)
+    if(identifiant==0x220)
     {
-        case 0x220:
-            for(int num_vent=0;num_vent<6;num_vent++)
-            {        
-                switch(VentEtat[num_vent])
+        for(int num_vent=0;num_vent<6;num_vent++)
+        {        
+            switch(VentEtat[num_vent])
+            {
+                case ETAT_ATTENDRE:
+                etat_groupe[num_vent] = msgRxBuffer[FIFO_ecriture].data[num_vent];
+                if(etat_groupe[num_vent] == 5) //Pompe et capteur
+                {
+                    VentEtat[num_vent] = ETAT_RECUP;
+                }
+                break;
+                
+                case ETAT_RECUP:
+                etat_groupe[num_vent] = msgRxBuffer[FIFO_ecriture].data[num_vent];
+                if(etat_groupe[num_vent] == 2) //Electrovane
                 {
-                    case ETAT_ATTENDRE:
-                    etat_groupe = msgRxBuffer[FIFO_ecriture].data[num_vent];
-                    if(etat_groupe == 5) //Pompe et capteur
-                    {
-                        VentEtat[num_vent] = ETAT_RECUP;
-                    }
-                    break;
-                    
-                    case ETAT_RECUP:
-                    etat_groupe = msgRxBuffer[FIFO_ecriture].data[num_vent];
-                    if(etat_groupe == 2) //Electrovane
-                    {
-                        VentEtat[num_vent] = ETAT_COMPTER;
-                    }
-                    break;
-                    
-                    case ETAT_COMPTER:
-                        verif_position_ventouse(num_vent);
-                        compteur_de_points();
-                        VentEtat[num_vent] = ETAT_ATTENDRE;
-                    break;
+                    VentEtat[num_vent] = ETAT_COMPTER;
                 }
-            }   
-        break;
+                break;
                 
-        case BRAS_RE:
-            num_groupe = msgRxBuffer[FIFO_ecriture].data[0];
-            verif_position_bras(num_groupe);
-            compteur_de_points();
-        break;
-                
-        case AUTOMATE_MANCHE_BAS:
-            num_groupe = msgRxBuffer[FIFO_ecriture].data[0];
-            verif_position_manche(num_groupe);
-            compteur_de_points();
-        break;
+                case ETAT_COMPTER:
+                    verif_position_ventouse(num_vent);
+                    compteur_de_points();
+                    VentEtat[num_vent] = ETAT_ATTENDRE;
+                break;
+            }
+        } 
+    }   
+        
+    if(Flag_Bras_Re == 1)
+    {
+        num_groupe = Flag_num_bras;
+        if(num_groupe <6) verif_position_bras(num_groupe);
+        else if(num_groupe == 10) {verif_position_bras(1); verif_position_bras(0);}
+        else if(num_groupe == 20) {verif_position_bras(2); verif_position_bras(0);}
+        else if(num_groupe == 21) {verif_position_bras(2); verif_position_bras(1);}
+        else if(num_groupe == 210){verif_position_bras(2); verif_position_bras(1); verif_position_bras(0);}
+        else if(num_groupe == 43) {verif_position_bras(4); verif_position_bras(3);}
+        else if(num_groupe == 53) {verif_position_bras(5); verif_position_bras(3);}
+        else if(num_groupe == 54) {verif_position_bras(5); verif_position_bras(4);}
+        else if(num_groupe == 66) {verif_position_bras(5); verif_position_bras(4); verif_position_bras(3);}
+        compteur_de_points();
+        Flag_Bras_Re = 0;
+    }
+    
+    if(Flag_Manche_Bas == 1)
+    {
+        verif_position_manche();
+        compteur_de_points();
+        Flag_Manche_Bas = 0;
     } 
+    
+    if(Flag_Manche_Moy == 1)
+    {
+        verif_position_phare();
+        compteur_de_points();
+        Flag_Manche_Moy = 0;    
+    }
+    
+    if(Flag_pavillon == 1)
+    {
+        pavillon = 1;
+        compteur_de_points();
+        Flag_pavillon = 0; 
+    }
+    
 }
 
 void compteur_de_points(void)
 {
+    if(Hauteur == 1) score_phare = 2; 
+    else if(Hauteur == 0) offset_score = 10;
+    
+    //Points gobelets
     int paire_ventouse = abs(gobelet_vert - gobelet_rouge);
     
-    //Points gobelets
-    score_ventouse = (gobelet_vert*2 + gobelet_rouge*2)*2 - (paire_ventouse)*2 + gobelet_port;
+    score_ventouse = (gobelet_vert + gobelet_rouge - paire_ventouse)*3 + paire_ventouse*2 + gobelet_port;
     
     //Points manche à air
     if(manche_releve == 1) score_manche = 5;
-    else if(manche_releve == 2) score_manche = 15;
+    else if(manche_releve >= 2) score_manche = 15;
     
     //Points phare
+    if(phare_active == 1) score_phare = 15;
     
     //Points pavillons
+    if(pavillon == 1) score_pavillon = 5;
     
     //Points d'arrivée à bon port
+    if(port_lu == 1 || port_lu == 2) score_bon_port = 10;
+    else score_bon_port = 3;
     
     //Points totaux
-    score_final = score_ventouse; //+ score_manche + score_phare + score_pavillon + score_bon_port;
+    score_final = score_ventouse + score_manche + score_pavillon + score_phare + score_bon_port;//+ offset_score;
 }
 
 
-
-
 /********************************************************************************************************************/
 /* FUNCTION NAME: verif_position_ventouse                                                                           */
 /* DESCRIPTION  : Vérifie si les gobelets posés par les ventouses sont dans un chenal/un port et augmente le score  */
 /********************************************************************************************************************/
 void verif_position_ventouse(int num_groupe)
 {   
-    int x_offset = Ventouse_coord[num_groupe][0]*cos(theta_robot*M_PI/1800) - Ventouse_coord[num_groupe][1]*sin(theta_robot*M_PI*1800);
-    int y_offset = Ventouse_coord[num_groupe][0]*sin(theta_robot*M_PI/1800) + Ventouse_coord[num_groupe][1]*cos(theta_robot*M_PI*1800);
+    float x_offset = Ventouse_coord[num_groupe][0]*cos((float)theta_robot*M_PI/1800.0f) - Ventouse_coord[num_groupe][1]*sin((float)theta_robot*M_PI/1800.0f);
+    float y_offset = Ventouse_coord[num_groupe][0]*sin((float)theta_robot*M_PI/1800.0f) + Ventouse_coord[num_groupe][1]*cos((float)theta_robot*M_PI/1800.0f);
     
     //chenal vert port départ bleu
-    if((x_robot+x_offset <= 530+DEMI_GOBELET)&&(x_robot+x_offset >= 500-DEMI_GOBELET)
-        && (y_robot+y_offset <= 400+DEMI_GOBELET)&&(y_robot+y_offset >= 0)) gobelet_vert++;
+    if(((float)x_robot+x_offset <= 530.0f+MARGE_VENT+ECART)&&((float)x_robot+x_offset >= 500.0f-MARGE_VENT-ECART)
+        && ((float)y_robot+y_offset <= 400.0f+MARGE_VENT)&&((float)y_robot+y_offset >= 0)) 
+        
+        {gobelet_vert++; Position_de_pose[num_groupe][0] = (float)x_robot+x_offset; Position_de_pose[num_groupe][1] = (float)y_robot+y_offset;}
+        
     //chenal rouge port départ bleu
-    else if((x_robot+x_offset <= 1100+DEMI_GOBELET)&&(x_robot+x_offset >= 1070-DEMI_GOBELET)
-            && (y_robot+y_offset <= 400+DEMI_GOBELET)&&(y_robot+y_offset >= 0)) gobelet_rouge++;
+    else if(((float)x_robot+x_offset <= 1100.0f+MARGE_VENT+ECART)&&((float)x_robot+x_offset >= 1070.0f-MARGE_VENT-ECART)
+            && ((float)y_robot+y_offset <= 400.0f+MARGE_VENT)&&((float)y_robot+y_offset >= 0))
+            
+        {gobelet_rouge++; Position_de_pose[num_groupe][0] = (float)x_robot+x_offset; Position_de_pose[num_groupe][1] = (float)y_robot+y_offset;}
+            
     //port départ bleu
-    else if((x_robot+x_offset > 530+DEMI_GOBELET)&&(x_robot+x_offset < 1070-DEMI_GOBELET)
-            && (y_robot+y_offset <= 400+DEMI_GOBELET)&&(y_robot+y_offset >= 0)) gobelet_port++;
+    else if(((float)x_robot+x_offset > 530.0f+MARGE_VENT+ECART)&&((float)x_robot+x_offset < 1070.0f-MARGE_VENT-ECART)
+            && ((float)y_robot+y_offset <= 400.0f+MARGE_VENT)&&((float)y_robot+y_offset >= 0)) 
+            
+            {gobelet_port++; Position_de_pose[num_groupe][0] = (float)x_robot+x_offset; Position_de_pose[num_groupe][1] = (float)y_robot+y_offset;}
             
     //chenal vert port ext bleu  
-    else if((x_robot+x_offset >= 1700-DEMI_GOBELET)&&(x_robot+x_offset <= 2000+DEMI_GOBELET)
-            && (y_robot+y_offset >= 1650-DEMI_GOBELET)&&(y_robot+y_offset <= 1750+DEMI_GOBELET)) gobelet_vert++;         
+    else if(((float)x_robot+x_offset >= 1700.0f-MARGE_VENT)&&((float)x_robot+x_offset <= 2000.0f+MARGE_VENT)
+            && ((float)y_robot+y_offset >= 1650.0f-MARGE_VENT)&&((float)y_robot+y_offset < 1800.0f)) 
+            
+            {gobelet_vert++; Position_de_pose[num_groupe][0] = (float)x_robot+x_offset; Position_de_pose[num_groupe][1] = (float)y_robot+y_offset;}
+                     
     //chenal rouge port ext bleu  
-    else if((x_robot+x_offset >= 1700-DEMI_GOBELET)&&(x_robot+x_offset <= 2000+DEMI_GOBELET)
-            && (y_robot+y_offset >= 1850-DEMI_GOBELET)&&(y_robot+y_offset <= 1950+DEMI_GOBELET)) gobelet_rouge++;
-    //port ext bleu  
-    else if((x_robot+x_offset >= 1700-DEMI_GOBELET)&&(x_robot+x_offset <= 2000+DEMI_GOBELET)
-            && (y_robot+y_offset > 1750+DEMI_GOBELET)&&(y_robot+y_offset < 1850-DEMI_GOBELET)) gobelet_port++;
+    else if(((float)x_robot+x_offset >= 1700.0f-MARGE_VENT)&&((float)x_robot+x_offset <= 2000.0f+MARGE_VENT)
+            && ((float)y_robot+y_offset >= 1800.0f)&&((float)y_robot+y_offset <= 1950.0f+MARGE_VENT)) 
+            
+            {gobelet_rouge++; Position_de_pose[num_groupe][0] = (float)x_robot+x_offset; Position_de_pose[num_groupe][1] = (float)y_robot+y_offset;}
             
             
     //chenal vert port départ jaune
-    else if((x_robot+x_offset <= 530+DEMI_GOBELET)&&(x_robot+x_offset >= 500-DEMI_GOBELET)
-            && (y_robot+y_offset <= 3000+DEMI_GOBELET)&&(y_robot+y_offset >= 2600-DEMI_GOBELET)) gobelet_vert++;
+    else if(((float)x_robot+x_offset <= 530.0f+MARGE_VENT+ECART)&&((float)x_robot+x_offset >= 500.0f-MARGE_VENT-ECART)
+            && ((float)y_robot+y_offset <= 3000.0f+MARGE_VENT)&&((float)y_robot+y_offset >= 2600.0f-MARGE_VENT)) 
+            
+            {gobelet_rouge++; Position_de_pose[num_groupe][0] = (float)x_robot+x_offset; Position_de_pose[num_groupe][1] = (float)y_robot+y_offset;}
+            
     //chenal rouge port départ jaune
-    else if((x_robot+x_offset <= 1100+DEMI_GOBELET)&&(x_robot+x_offset >= 1070-DEMI_GOBELET)
-            && (y_robot+y_offset <= 3000+DEMI_GOBELET)&&(y_robot+y_offset >= 2600-DEMI_GOBELET)) gobelet_rouge++;
+    else if(((float)x_robot+x_offset <= 1100.0f+MARGE_VENT+ECART)&&((float)x_robot+x_offset >= 1070.0f-MARGE_VENT-ECART)
+            && ((float)y_robot+y_offset <= 3000.0f+MARGE_VENT)&&((float)y_robot+y_offset >= 2600.0f-MARGE_VENT)) 
+            
+            {gobelet_vert++; Position_de_pose[num_groupe][0] = (float)x_robot+x_offset; Position_de_pose[num_groupe][1] = (float)y_robot+y_offset;}
+            
     //port départ jaune
-    else if((x_robot+x_offset > 530+DEMI_GOBELET)&&(x_robot+x_offset < 1070-DEMI_GOBELET)
-            && (y_robot+y_offset <= 3000+DEMI_GOBELET)&&(y_robot+y_offset >= 2600-DEMI_GOBELET)) gobelet_port++;
+    else if(((float)x_robot+x_offset > 530.0f+MARGE_VENT+ECART)&&((float)x_robot+x_offset < 1070.0f-MARGE_VENT-ECART)
+            && ((float)y_robot+y_offset <= 3000.0f+MARGE_VENT)&&((float)y_robot+y_offset >= 2600.0f-MARGE_VENT)) 
+            
+            {gobelet_port++; Position_de_pose[num_groupe][0] = (float)x_robot+x_offset; Position_de_pose[num_groupe][1] = (float)y_robot+y_offset;}
             
     //chenal vert port ext jaune  
-    else if((x_robot+x_offset >= 1700-DEMI_GOBELET)&&(x_robot+x_offset <= 2000+DEMI_GOBELET)
-            && (y_robot+y_offset >= 1050-DEMI_GOBELET)&&(y_robot+y_offset <= 1150+DEMI_GOBELET)) gobelet_vert++;               
+    else if(((float)x_robot+x_offset >= 1700.0f-MARGE_VENT)&&((float)x_robot+x_offset <= 2000.0f+MARGE_VENT)
+            && ((float)y_robot+y_offset >= 1050.0f-MARGE_VENT)&&((float)y_robot+y_offset < 1200.0f)) 
+            
+            {gobelet_vert++; Position_de_pose[num_groupe][0] = (float)x_robot+x_offset; Position_de_pose[num_groupe][1] = (float)y_robot+y_offset;}
+                           
     //chenal rouge port ext jaune  
-    else if((x_robot+x_offset >= 1700-DEMI_GOBELET)&&(x_robot+x_offset <= 2000+DEMI_GOBELET)
-            && (y_robot+y_offset >= 1250-DEMI_GOBELET)&&(y_robot+y_offset <= 1350+DEMI_GOBELET)) gobelet_rouge++;
-    //port ext jaune  
-    else if((x_robot+x_offset >= 1700-DEMI_GOBELET)&&(x_robot+x_offset <= 2000+DEMI_GOBELET)
-            && (y_robot+y_offset > 1150+DEMI_GOBELET)&&(y_robot+y_offset < 1250-DEMI_GOBELET)) gobelet_port++;
+    else if(((float)x_robot+x_offset >= 1700.0f-MARGE_VENT)&&((float)x_robot+x_offset <= 2000.0f+MARGE_VENT)
+            && ((float)y_robot+y_offset >= 1200.0f)&&((float)y_robot+y_offset <= 1350.0f+MARGE_VENT)) 
+            
+            {gobelet_rouge++; Position_de_pose[num_groupe][0] = (float)x_robot+x_offset; Position_de_pose[num_groupe][1] = (float)y_robot+y_offset;}
 }
 
+/********************************************************************************************************************/
+/* FUNCTION NAME: verif_position_bras                                                                               */
+/* DESCRIPTION  : Vérifie si les gobelets posés par les bras sont dans un chenal/un port et augmente le score       */
+/********************************************************************************************************************/
 void verif_position_bras(int num_groupe)
-{   
+{
+    float x_offset = Bras_coord[num_groupe][0]*cos((float)theta_robot*M_PI/1800.0f) - Bras_coord[num_groupe][1]*sin((float)theta_robot*M_PI/1800.0f);
+    float y_offset = Bras_coord[num_groupe][0]*sin((float)theta_robot*M_PI/1800.0f) + Bras_coord[num_groupe][1]*cos((float)theta_robot*M_PI/1800.0f);
+    
+    
+    //chenal vert port départ bleu
+    if(((float)x_robot+x_offset <= 530.0f+MARGE_BRAS+ECART)&&((float)x_robot+x_offset >= 500.0f-MARGE_BRAS-ECART)
+        && ((float)y_robot+y_offset <= 400.0f+MARGE_BRAS)&&((float)y_robot+y_offset >= 0)) gobelet_vert++;
+    //chenal rouge port départ bleu
+    else if(((float)x_robot+x_offset <= 1100.0f+MARGE_BRAS+ECART)&&((float)x_robot+x_offset >= 1070.0f-MARGE_BRAS-ECART)
+            && ((float)y_robot+y_offset <= 400.0f+MARGE_BRAS)&&((float)y_robot+y_offset >= 0)) gobelet_rouge++;
+    //port départ bleu
+    else if(((float)x_robot+x_offset > 530.0f+MARGE_BRAS+ECART)&&((float)x_robot+x_offset < 1070.0f-MARGE_BRAS-ECART)
+            && ((float)y_robot+y_offset <= 400.0f+MARGE_BRAS)&&((float)y_robot+y_offset >= 0)) gobelet_port++;
+            
+    //chenal vert port ext bleu  
+    else if(((float)x_robot+x_offset >= 1700.0f-MARGE_BRAS)&&((float)x_robot+x_offset <= 2000.0f+MARGE_BRAS)
+            && ((float)y_robot+y_offset >= 1650.0f-MARGE_BRAS)&&((float)y_robot+y_offset <= 1780.0f)) gobelet_vert++;         
+    //chenal rouge port ext bleu  
+    else if(((float)x_robot+x_offset >= 1700.0f-MARGE_BRAS)&&((float)x_robot+x_offset <= 2000.0f+MARGE_BRAS)
+            && ((float)y_robot+y_offset >= 1820.0f)&&((float)y_robot+y_offset <= 1950.0f+MARGE_BRAS)) gobelet_rouge++;
+    //Port ext bleu
+    else if((x_robot+x_offset >= 1700.0f-MARGE_BRAS)&&(x_robot+x_offset <= 2000.0f+MARGE_BRAS)
+            && (y_robot+y_offset > 1780.0f)&&(y_robot+y_offset < 1820.0f)) gobelet_port++;
+            
+            
+    //chenal vert port départ jaune
+    else if(((float)x_robot+x_offset <= 530.0f+MARGE_BRAS+ECART)&&((float)x_robot+x_offset >= 500.0f-MARGE_BRAS-ECART)
+            && ((float)y_robot+y_offset <= 3000.0f+MARGE_BRAS)&&((float)y_robot+y_offset >= 2600.0f-MARGE_BRAS)) gobelet_rouge++;
+    //chenal rouge port départ jaune
+    else if(((float)x_robot+x_offset <= 1100.0f+MARGE_BRAS+ECART)&&((float)x_robot+x_offset >= 1070.0f-MARGE_BRAS-ECART)
+            && ((float)y_robot+y_offset <= 3000.0f+MARGE_BRAS)&&((float)y_robot+y_offset >= 2600.0f-MARGE_BRAS)) gobelet_vert++;
+    //port départ jaune
+    else if(((float)x_robot+x_offset > 530.0f+MARGE_BRAS+ECART)&&((float)x_robot+x_offset < 1070.0f-MARGE_BRAS-ECART)
+            && ((float)y_robot+y_offset <= 3000.0f+MARGE_BRAS)&&((float)y_robot+y_offset >= 2600.0f-MARGE_BRAS)) gobelet_port++;
+            
+    //chenal vert port ext jaune  
+    else if(((float)x_robot+x_offset >= 1700.0f-MARGE_BRAS)&&((float)x_robot+x_offset <= 2000.0f+MARGE_BRAS)
+            && ((float)y_robot+y_offset >= 1050.0f-MARGE_BRAS)&&((float)y_robot+y_offset <= 1180.0f)) gobelet_vert++;               
+    //chenal rouge port ext jaune  
+    else if(((float)x_robot+x_offset >= 1700.0f-MARGE_BRAS)&&((float)x_robot+x_offset <= 2000.0f+MARGE_BRAS)
+            && ((float)y_robot+y_offset >= 1220.0f)&&((float)y_robot+y_offset <= 1350.0f+MARGE_BRAS)) gobelet_rouge++;
+    //Port ext jaune
+    else if((float)(x_robot+x_offset >= 1700.0f-MARGE_BRAS)&&((float)x_robot+x_offset <= 2000.0f+MARGE_BRAS)
+            && ((float)y_robot+y_offset > 1180.0f)&&((float)y_robot+y_offset < 1220.0f)) gobelet_port++;
 
 }
 
-void verif_position_manche(int num_groupe)
-{    
+/********************************************************************************************************************/
+/* FUNCTION NAME: verif_position_manche                                                                             */
+/* DESCRIPTION  : Vérifie si les bras des manches à air sont déployés au bon enroit et augmente le score            */
+/********************************************************************************************************************/
+void verif_position_manche(void)
+{
+    //Manches Bleus
+    if((x_robot <= 2000+MARGE_VENT)&&(x_robot >= 1700-MARGE_VENT)
+        && (y_robot <= 890+MARGE_VENT)&&(y_robot >= 0)) manche_releve++;
+        
+    //Manches Jaunes 
+    else if((x_robot <= 2000+MARGE_VENT)&&(x_robot >= 1700-MARGE_VENT)
+            && (y_robot <= 3000)&&(y_robot >= 2090-MARGE_VENT)) manche_releve++;
+}
 
+/********************************************************************************************************************/
+/* FUNCTION NAME: verif_position_phare                                                                            */
+/* DESCRIPTION  : Vérifie si les bras des manches à air sont déployés au bon enroit et augmente le score            */
+/********************************************************************************************************************/
+void verif_position_phare(void)
+{
+    //Phare Bleu
+    if((x_robot <= 300+MARGE_VENT)&&(x_robot >= 0-MARGE_VENT)
+        && (y_robot <= 890+MARGE_VENT)&&(y_robot >= 0)) phare_active=1;
+    
+    //Phare Jaune
+    else if((x_robot <= 300+MARGE_VENT)&&(x_robot >= 0-MARGE_VENT)
+            && (y_robot <= 3000)&&(y_robot >= 2090-MARGE_VENT)) phare_active=1;
 }
\ No newline at end of file
--- a/Compteur_points/Compteur.h	Mon May 31 13:36:03 2021 +0000
+++ b/Compteur_points/Compteur.h	Sat Jul 17 11:07:17 2021 +0000
@@ -11,13 +11,28 @@
 
 void verif_position_bras(int num_groupe);
 
-void verif_position_manche(int num_groupe);
+void verif_position_manche(void);
 
+void verif_position_phare(void);
+
+extern int etat_groupe[6];
 extern int score_final;
 extern int gobelet_vert;
 extern int gobelet_rouge;
 extern int gobelet_port;
 
+extern int manche_releve;
+extern int phare_active;
+extern int pavillon;
+extern int port_lu;
+
+extern int old_gobelet_vert, old_gobelet_rouge, old_gobelet_port;
+
+extern float Position_de_pose[6][2];
+
+
+extern unsigned char num_groupe;
+
 typedef enum
 {
   ETAT_ATTENDRE,
--- a/Evitement/Evitement.cpp	Mon May 31 13:36:03 2021 +0000
+++ b/Evitement/Evitement.cpp	Sat Jul 17 11:07:17 2021 +0000
@@ -25,10 +25,56 @@
 /* FUNCTION NAME: Balise Danger                                                         */
 /* DESCRIPTION  : FIFO -> BALISE_DANGER                                                 */
 /****************************************************************************************/
-unsigned short balise_danger(void){ 
-    SendSpeed(150);
-    return(0);
+
+int Flag_stoped_danger = 0;
+
+Ticker ticker_timeout_evitement;
+
+unsigned short balise_danger(signed char FIFO_lecture)
+{ 
+    /*signed char fin_angle_detection;
+    signed char debut_angle_detection;
+    float angle_moyen_balise_IR = 0.0;
+//    Debug_Audio(3,2);
+    //on recupere l'info d'angle de detection--------------------------------------
+   if(msgRxBuffer[FIFO_lecture].data[0]!=0) { //data balise Petit Robot Detecte
+        fin_angle_detection = msgRxBuffer[FIFO_lecture].data[0] & 0x0F;
+        debut_angle_detection = (msgRxBuffer[FIFO_lecture].data[0] & 0xF0) >> 4;
+    } else { //data balise Gros Robot Detecte
+        fin_angle_detection = msgRxBuffer[FIFO_lecture].data[2] & 0x0F;
+        debut_angle_detection = (msgRxBuffer[FIFO_lecture].data[2] & 0xF0) >> 4;
+    }
+    //on moyenne l'angle------------------------------------------------------------
+    if(debut_angle_detection > fin_angle_detection) {
+        angle_moyen_balise_IR = (float)debut_angle_detection + ((15.0f-(float)debut_angle_detection)+(float)fin_angle_detection)/2.0f;
+        if(angle_moyen_balise_IR > 15.0f)
+            angle_moyen_balise_IR-=15.0f;
+    } else
+        angle_moyen_balise_IR = debut_angle_detection + (fin_angle_detection-debut_angle_detection)/2;
+    char seuil_bas_arriere = 5; 
+    char seuil_haut_arriere = 7;
+    char seuil_bas_avant = 0xF;
+    char seuil_haut_avant = 0xD;
+ //   #endif 
+    // LARNAUDIE 3/7/2021 if((angle_moyen_balise_IR>=seuil_bas_avant && angle_moyen_balise_IR<=seuil_haut_avant)) // || (angle_moyen_balise_IR>=seuil_bas_arriere && angle_moyen_balise_IR<=seuil_haut_arriere))
+    if((actionPrecedente==MV_COURBURE)||(actionPrecedente==MV_LINE)||(actionPrecedente==MV_XYT))
+    {
+        if(asser_stop_direction==1)
+        {
+            if( ((debut_angle_detection>=seuil_haut_avant) && (debut_angle_detection<=seuil_bas_avant))
+                ||((fin_angle_detection>=seuil_haut_avant) && (fin_angle_detection<=seuil_bas_avant))   )
+                SendRawId(ASSERVISSEMENT_STOP);
+        }
+        else
+        {
+            if( ((debut_angle_detection>=seuil_bas_arriere) && (debut_angle_detection<=seuil_haut_arriere))
+                ||((fin_angle_detection>=seuil_bas_arriere) && (fin_angle_detection<=seuil_haut_arriere))   )
+                SendRawId(ASSERVISSEMENT_STOP);
+        }
+    }
+    return(0);*/
 }
+
 /****************************************************************************************/
 /* FUNCTION NAME: Balise Stop                                                           */
 /* DESCRIPTION  : FIFO -> BALISE_STOP                                                   */
@@ -38,9 +84,9 @@
     signed char fin_angle_detection;
     signed char debut_angle_detection;
     float angle_moyen_balise_IR = 0.0;
-    Debug_Audio(3,2);
+//    Debug_Audio(3,2);
     //on recupere l'info d'angle de detection--------------------------------------
-    if(msgRxBuffer[FIFO_lecture].data[0]!=0) { //data balise Petit Robot Detecte
+   if(msgRxBuffer[FIFO_lecture].data[0]!=0) { //data balise Petit Robot Detecte
         fin_angle_detection = msgRxBuffer[FIFO_lecture].data[0] & 0x0F;
         debut_angle_detection = (msgRxBuffer[FIFO_lecture].data[0] & 0xF0) >> 4;
     } else { //data balise Gros Robot Detecte
@@ -54,84 +100,155 @@
             angle_moyen_balise_IR-=15.0f;
     } else
         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 = 15.0;
-    float seuil_bas_arriere = 5.0;
-    float seuil_haut_arriere = 7.0;
-    #else 
-    float seuil_bas_arriere = 12.0; 
-    float seuil_haut_arriere = 15.0;
-    float seuil_bas_avant = 4.0;
-    float seuil_haut_avant = 7.0;
-    #endif 
-    if((angle_moyen_balise_IR>=seuil_bas_avant && angle_moyen_balise_IR<=seuil_haut_avant)) // || (angle_moyen_balise_IR>=seuil_bas_arriere && angle_moyen_balise_IR<=seuil_haut_arriere))
-        SendRawId(ASSERVISSEMENT_STOP);
+    char seuil_bas_arriere = 5; 
+    char seuil_haut_arriere = 7;
+    char seuil_bas_avant = 0xF;
+    char seuil_haut_avant = 0xD;
+ //   #endif 
+    // LARNAUDIE 3/7/2021 if((angle_moyen_balise_IR>=seuil_bas_avant && angle_moyen_balise_IR<=seuil_haut_avant)) // || (angle_moyen_balise_IR>=seuil_bas_arriere && angle_moyen_balise_IR<=seuil_haut_arriere))
+    if((actionPrecedente==MV_COURBURE)||(actionPrecedente==MV_LINE)||(actionPrecedente==MV_XYT))
+    {
+        if(Cote == 1)//Jaune
+        {
+            if((x_robot >= 1600 && y_robot >= 2200) || (y_robot >= 2500) || (x_robot >= 1750 && y_robot >= 1000 && y_robot <= 1400)) {}
+            else
+            {
+                if(asser_stop_direction==1)
+                {
+                    if( ((debut_angle_detection>=seuil_haut_avant) && (debut_angle_detection<=seuil_bas_avant))
+                        ||((fin_angle_detection>=seuil_haut_avant) && (fin_angle_detection<=seuil_bas_avant))   )
+                        SendRawId(ASSERVISSEMENT_STOP);
+                            Flag_stoped_danger = 1;
+                }
+                else
+                {
+                    if( ((debut_angle_detection>=seuil_bas_arriere) && (debut_angle_detection<=seuil_haut_arriere))
+                        ||((fin_angle_detection>=seuil_bas_arriere) && (fin_angle_detection<=seuil_haut_arriere))   )
+                        SendRawId(ASSERVISSEMENT_STOP);
+                            Flag_stoped_danger = 1;
+                }
+            }
+        }   
+        
+        else if(Cote == 0)//Bleu
+        {
+            if((x_robot >= 1600 && y_robot <= 800) || (y_robot <= 500) || (x_robot >= 1750 && y_robot >= 1600 && y_robot <= 2000)) {}
+            else
+            {
+                if(asser_stop_direction==1)
+                {
+                    if( ((debut_angle_detection>=seuil_haut_avant) && (debut_angle_detection<=seuil_bas_avant))
+                        ||((fin_angle_detection>=seuil_haut_avant) && (fin_angle_detection<=seuil_bas_avant))   )
+                        SendRawId(ASSERVISSEMENT_STOP);
+                            Flag_stoped_danger = 1;
+                }
+                else
+                {
+                    if( ((debut_angle_detection>=seuil_bas_arriere) && (debut_angle_detection<=seuil_haut_arriere))
+                        ||((fin_angle_detection>=seuil_bas_arriere) && (fin_angle_detection<=seuil_haut_arriere))   )
+                        SendRawId(ASSERVISSEMENT_STOP);
+                            Flag_stoped_danger = 1;
+                }
+            }
+        }
+        if(Flag_stoped_danger)
+        {
+            ticker_timeout_evitement.detach();
+            ticker_timeout_evitement.attach(&isr_end_danger, 2);
+        }
+    }
     return(0);
 }
 /****************************************************************************************/
 /* FUNCTION NAME: Balise end Danger                                                     */
 /* DESCRIPTION  : FIFO -> BALISE_END_DANGER                                             */
 /****************************************************************************************/
-unsigned short balise_end_danger(S_Instruction* instruction,S_Dodge_queue* dodgeq, signed short target_x_robot, signed short target_y_robot, signed short target_theta_robot, signed short theta_robot,signed short x_robot,signed short y_robot){
-    Debug_Audio(3,1);
-    switch(instruction->order){
-        case MV_LINE:
-            gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;
-            instruction->order = MV_XYT;
-            instruction->arg1 = target_x_robot;// X
-            instruction->arg2 = target_y_robot;// Y
-            instruction->arg3 = target_theta_robot;// T
-        break;
-        case MV_TURN:
-            gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;
-            instruction->order = MV_XYT;
-            instruction->arg1 = target_x_robot;// X
-            instruction->arg2 = target_y_robot;// Y
-            instruction->arg3 = target_theta_robot;// T
-        break;
-        case MV_XYT:
-            gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;
-        break;
-        case MV_COURBURE:
-            unsigned short alpha;
-            gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;
-            instruction->order=MV_XYT;
-            if(instruction->direction==LEFT) alpha=(dodgeq->inst[0].arg3-theta_robot);
-            else alpha=(theta_robot-dodgeq->inst[0].arg3);   
-            if(alpha<450){
-                dodgeq->nb=0;
-                instruction->arg1=dodgeq->inst[0].arg1;//x
-                instruction->arg2=dodgeq->inst[0].arg2;//y
-                instruction->arg3=dodgeq->inst[0].arg3;//t
-            } else if(alpha<900){ 
-                dodgeq->nb=1;
-                instruction->arg1=dodgeq->inst[1].arg1;//x
-                instruction->arg2=dodgeq->inst[1].arg2;//y
-                instruction->arg3=dodgeq->inst[1].arg3;//t
-            } else if(alpha<1350){ 
-                dodgeq->nb=2;
-                instruction->arg1=dodgeq->inst[2].arg1;//x
-                instruction->arg2=dodgeq->inst[2].arg2;//y
-                instruction->arg3=dodgeq->inst[2].arg3;//t
-            } else if(alpha<1800){ 
-                dodgeq->nb=3;
-                instruction->arg1=dodgeq->inst[3].arg1;//x
-                instruction->arg2=dodgeq->inst[3].arg2;//y
-                instruction->arg3=dodgeq->inst[3].arg3;//t
-            } else if(alpha<2250){ 
-                dodgeq->nb=4;
-                instruction->arg1=dodgeq->inst[4].arg1;//x
-                instruction->arg2=dodgeq->inst[4].arg2;//y
-                instruction->arg3=dodgeq->inst[4].arg3;//t
-            } else { 
-                dodgeq->nb=5;
-                instruction->arg1=dodgeq->inst[5].arg1;//x
-                instruction->arg2=dodgeq->inst[5].arg2;//y
-                instruction->arg3=dodgeq->inst[5].arg3;//t
-            } 
-        break;
+unsigned short balise_end_danger(S_Instruction* instruction,S_Dodge_queue* dodgeq, signed short local_target_x_robot, signed short local_target_y_robot, signed short local_target_theta_robot, signed short local_theta_robot,signed short x_robot,signed short y_robot){
+    
+    ticker_timeout_evitement.detach();
+    if(Flag_stoped_danger)
+    {
+        
+        ingnorInversionOnce=1;
+        wait_ms(1000);
+        switch(instruction->order){
+            case MV_RECALAGE:
+                gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;  
+                    Flag_stoped_danger = 0;          
+            break;
+            
+            case MV_LINE:
+                gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;
+                instruction->order = MV_XYT;
+                instruction->arg1 = local_target_x_robot;// X
+                instruction->arg2 = local_target_y_robot;// Y
+                instruction->arg3 = local_target_theta_robot;// T
+                    Flag_stoped_danger = 0;
+            break;
+            case MV_TURN:
+                gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;
+                instruction->order = MV_XYT;
+                instruction->arg1 = local_target_x_robot;// X
+                instruction->arg2 = local_target_y_robot;// Y
+                instruction->arg3 = local_target_theta_robot;// T
+                    Flag_stoped_danger = 0;
+            break;
+            case MV_XYT:
+                gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;
+                    Flag_stoped_danger = 0;
+            break;
+            case MV_COURBURE:
+                short alpha;
+                gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;
+                instruction->order=MV_XYT;
+                if(instruction->direction==LEFT) 
+                    alpha=(dodgeq->inst[0].arg3-theta_robot);
+                else 
+                    alpha=(theta_robot-dodgeq->inst[0].arg3);
+                if(alpha>3600)
+                    alpha=alpha-3600;
+                if(alpha<-3600)
+                    alpha=alpha+3600;   
+                if(alpha<0)
+                    alpha=-alpha;
+                if(alpha<450)
+                {
+                    dodgeq->nb=0;
+                    instruction->arg1=dodgeq->inst[0].arg1;//x
+                    instruction->arg2=dodgeq->inst[0].arg2;//y
+                    instruction->arg3=dodgeq->inst[0].arg3;//t
+                } 
+                else if(alpha<900)
+                { 
+                    dodgeq->nb=1;
+                    instruction->arg1=dodgeq->inst[1].arg1;//x
+                    instruction->arg2=dodgeq->inst[1].arg2;//y
+                    instruction->arg3=dodgeq->inst[1].arg3;//t
+                } else if(alpha<1350){ 
+                    dodgeq->nb=2;
+                    instruction->arg1=dodgeq->inst[2].arg1;//x
+                    instruction->arg2=dodgeq->inst[2].arg2;//y
+                    instruction->arg3=dodgeq->inst[2].arg3;//t
+                } else if(alpha<1800){ 
+                    dodgeq->nb=3;
+                    instruction->arg1=dodgeq->inst[3].arg1;//x
+                    instruction->arg2=dodgeq->inst[3].arg2;//y
+                    instruction->arg3=dodgeq->inst[3].arg3;//t
+                } else if(alpha<2250){ 
+                    dodgeq->nb=4;
+                    instruction->arg1=dodgeq->inst[4].arg1;//x
+                    instruction->arg2=dodgeq->inst[4].arg2;//y
+                    instruction->arg3=dodgeq->inst[4].arg3;//t
+                } else { 
+                    dodgeq->nb=5;
+                    instruction->arg1=dodgeq->inst[5].arg1;//x
+                    instruction->arg2=dodgeq->inst[5].arg2;//y
+                    instruction->arg3=dodgeq->inst[5].arg3;//t
+                } 
+                    Flag_stoped_danger = 0;
+            break;
         }
-    SendSpeed(300);
+//    SendSpeed(300);
+    }
     return(0);
 }
--- a/Evitement/Evitement.h	Mon May 31 13:36:03 2021 +0000
+++ b/Evitement/Evitement.h	Sat Jul 17 11:07:17 2021 +0000
@@ -9,7 +9,7 @@
 #define T_X 2000
 #define T_Y 3000
 
-unsigned short balise_danger(void);
+unsigned short balise_danger(signed char FIFO_lecture);
 unsigned short balise_stop(signed char FIFO_lecture);
 unsigned short balise_end_danger(S_Instruction* instruction,S_Dodge_queue* dodgeq, signed short target_x_robot, signed short target_y_robot, signed short target_theta_robot, signed short theta_robot,signed short x_robot,signed short y_robot);
 
--- a/Globals/global.h	Mon May 31 13:36:03 2021 +0000
+++ b/Globals/global.h	Sat Jul 17 11:07:17 2021 +0000
@@ -24,8 +24,10 @@
 #include "Evitement.h"
 #include "ihm.h"
 #include "Compteur.h"
-
-
+#include "bon_port.h"
+extern unsigned char ingnorInversionOnce;
+extern signed char asser_stop_direction;
+extern E_InstructionType actionPrecedente;
 extern Serial pc;
 //extern E_stratGameEtat gameEtat = ETAT_CHECK_CARTES;
 //extern T_etat strat_etat_s = INIT;
--- a/Globals/ident_crac.lib	Mon May 31 13:36:03 2021 +0000
+++ b/Globals/ident_crac.lib	Sat Jul 17 11:07:17 2021 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/teams/CRAC-Team/code/ident_crac/#3cdea2100f98
+https://os.mbed.com/teams/CRAC-Team/code/ident_crac/#8efe164137c7
--- a/IHM/ihm.cpp	Mon May 31 13:36:03 2021 +0000
+++ b/IHM/ihm.cpp	Sat Jul 17 11:07:17 2021 +0000
@@ -40,6 +40,10 @@
 Button TEST_NEXT(0,575,400,100,"------>");
 Button TEST_DIV(0,25,400,100,"Test Divers");
 Button TEST_ASSERV(0, 135, 400, 100, "Test asserv");
+Button TEST_MANCHE(0,245,400,100,"Test manches a air");
+Button TEST_GIROU(0,355,400,100,"Test capteur girouette");
+
+
 //menu demo/demo/
 Button TEST_A(0,25,195,100,"A");
 Button TEST_B(205,25,195,100,"B");
@@ -51,19 +55,19 @@
 
 Button TEST_PLUS(205,355,195,100,"+");
 Button TEST_MOINS(0,355,195,100,"-");
-Button TEST_SERVO_0(280,50,120,100,"Bras 0");
-Button TEST_SERVO_1(140,50,120,100,"Bras 1");
-Button TEST_SERVO_2(0,50,120,100,"Bras 2");
-Button TEST_SERVO_3(0,160,120,100,"Bras 3");
-Button TEST_SERVO_4(140,160,120,100,"Bras 4");
-Button TEST_SERVO_5(280,160,120,100,"Bras 5");
+Button TEST_SERVO_0(0,160,120,100,"Bras 0");
+Button TEST_SERVO_1(140,160,120,100,"Bras 1");
+Button TEST_SERVO_2(280,160,120,100,"Bras 2");
+Button TEST_SERVO_3(280,50,120,100,"Bras 3");
+Button TEST_SERVO_4(140,50,120,100,"Bras 4");
+Button TEST_SERVO_5(0,50,120,100,"Bras 5");
 
-Button TEST_VENT_0(280,50,120,100,"Vent 0");
-Button TEST_VENT_1(140,50,120,100,"Vent 1");
-Button TEST_VENT_2(0,50,120,100,"Vent 2");
-Button TEST_VENT_3(0,160,120,100,"Vent 3");
-Button TEST_VENT_4(140,160,120,100,"Vent 4");
-Button TEST_VENT_5(280,160,120,100,"Vent 5");
+Button TEST_VENT_0(0,160,120,100,"Vent 0");
+Button TEST_VENT_1(140,160,120,100,"Vent 1");
+Button TEST_VENT_2(280,160,120,100,"Vent 2");
+Button TEST_VENT_3(280,50,120,100,"Vent 3");
+Button TEST_VENT_4(140,50,120,100,"Vent 4");
+Button TEST_VENT_5(0,50,120,100,"Vent 5");
 
 Button FORCE_LAUNCH(0, 600, 400, 100, "Force Launch");
 Button SUIVANT(0,380,200,100,"Suivant");
@@ -81,7 +85,7 @@
 /* DESCRIPTION  : Automate de gestion de l'affichage                                    */
 /****************************************************************************************/
 void automate_etat_ihm(void)
-{
+{            
     char toto[12]; 
     int j, old_time=0, actual_time=0, old_score=0, actual_score=0, unique=0;
     unsigned char maximilien=1, choix_groupe;
@@ -167,25 +171,38 @@
             maximilien=0;
             while (strat_etat_s == DEMO) {
                 canProcessRx();
-                if(TEST_VENT.Touched()) {
+                if(TEST_VENT.Touched()) 
+                {
                     while(TEST_VENT.Touched());
                     strat_etat_s = TEST_VENTOUSE;
-                } else if(TEST_MOT.Touched()) {
+                } 
+                else if(TEST_MOT.Touched()) 
+                {
                     while(TEST_MOT.Touched());
                     strat_etat_s = TEST_MOTEUR;
-                } else if(TEST_NEXT.Touched()) {
+                } 
+                else if(TEST_NEXT.Touched()) 
+                {
                     while(TEST_NEXT.Touched());
                     strat_etat_s = DEMO2;
-                } else if (TEST_COUL.Touched()) {
+                } 
+                else if (TEST_COUL.Touched()) 
+                {
                     while(TEST_COUL.Touched());
                     strat_etat_s =TEST_COULEUR ;
-                } else if (TEST_BRAS.Touched()) {
+                } 
+                else if (TEST_BRAS.Touched()) 
+                {
                     while(TEST_BRAS.Touched());
                     strat_etat_s =TEST_SERVO_BRAS ;
-                } else if(TEST_AUDIO.Touched()) {
+                } 
+                else if(TEST_AUDIO.Touched()) 
+                {
                     while(TEST_AUDIO.Touched());
                     strat_etat_s=TEST_AUD;
-                } else if(RETOUR.Touched()) {
+                } 
+                else if(RETOUR.Touched()) 
+                {
                     while(RETOUR.Touched());
                     strat_etat_s = CHOIX;
                 }
@@ -200,26 +217,119 @@
             RETOUR.Draw(0xFFFF0000, 0);
             TEST_DIV.Draw(VERT, 0);
             TEST_ASSERV.Draw(VERT, 0);
+            TEST_MANCHE.Draw(VERT, 0);
+            TEST_GIROU.Draw(VERT, 0);
             TEST_NEXT.Draw(BLEU, 0);
             maximilien=0;
             while (strat_etat_s == DEMO2) {
                 canProcessRx();
-                if(TEST_NEXT.Touched()) {
+                if(TEST_NEXT.Touched()) 
+                {
                     while(TEST_NEXT.Touched());
                     strat_etat_s = DEMO;
-                } else if(TEST_DIV.Touched()) {
+                } 
+                else if(TEST_DIV.Touched()) 
+                {
                     while(TEST_DIV.Touched());
                     strat_etat_s = TEST_DIVE;
-                } else if(TEST_ASSERV.Touched()) {
+                } 
+                else if(TEST_ASSERV.Touched()) 
+                {
                     while(TEST_ASSERV.Touched());
                     strat_etat_s = TEST_ASSERVE;
-                } else if(RETOUR.Touched()) {
+                } 
+                else if(TEST_MANCHE.Touched()) 
+                {
+                    while(TEST_MANCHE.Touched());
+                    strat_etat_s = TEST_MANCHES;
+                }
+                else if(TEST_GIROU.Touched()) 
+                {
+                    while(TEST_GIROU.Touched());
+                    strat_etat_s = TEST_GIROUS;
+                }
+                
+                else if(RETOUR.Touched()) 
+                {
                     while(RETOUR.Touched());
                     strat_etat_s = CHOIX;
                 }
             }
         break;
         
+        case TEST_MANCHES:
+            lcd.SetBackColor(LCD_COLOR_WHITE);
+            lcd.SetTextColor(LCD_COLOR_BLACK);
+            lcd.Clear (LCD_COLOR_WHITE);
+            lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"Manches a air", LEFT_MODE);
+            
+            RETOUR.Draw(0xFFFF0000,0);
+            TEST_A.Draw(BLEU, BLANC);
+            TEST_B.Draw(BLEU, BLANC);   
+            TEST_C.Draw(BLEU, BLANC);  
+            TEST_D.Draw(BLEU, BLANC) ;
+            
+            while(strat_etat_s==TEST_MANCHES) 
+            {
+                if(RETOUR.Touched()) 
+                {
+                    while (RETOUR.Touched());
+                    strat_etat_s=DEMO2;
+                }
+                else if(TEST_A.Touched()) 
+                {
+                    while (TEST_A.Touched());
+                    TEST_A.Draw(BLEU, BLANC);
+                    SendRawId(TEST_BRAS_1);
+                }  
+                else if(TEST_B.Touched()) 
+                {
+                    while (TEST_B.Touched());
+                    TEST_B.Draw(BLEU, BLANC); 
+                    choix_groupe = 0;
+                    SendMsgCan(AUTOMATE_MANCHE_MOY, &choix_groupe, sizeof(choix_groupe)) ;   
+                    wait_ms(500);
+                    SendMsgCan(AUTOMATE_MANCHE_BAS, &choix_groupe, sizeof(choix_groupe)) ;   
+                    wait_ms(500);
+                    SendMsgCan(AUTOMATE_MANCHE_HAUT, &choix_groupe, sizeof(choix_groupe)) ;   
+                    wait_ms(500);
+                    choix_groupe = 1;
+                    SendMsgCan(AUTOMATE_MANCHE_MOY, &choix_groupe, sizeof(choix_groupe)) ;   
+                    wait_ms(500);
+                    SendMsgCan(AUTOMATE_MANCHE_BAS, &choix_groupe, sizeof(choix_groupe)) ;   
+                    wait_ms(500);
+                    SendMsgCan(AUTOMATE_MANCHE_HAUT, &choix_groupe, sizeof(choix_groupe)) ;   
+                    wait_ms(500);
+                }
+                 else if(TEST_C.Touched()) 
+                {
+                    while (TEST_A.Touched());
+                    TEST_C.Draw(BLEU, BLANC);
+                    SendRawId(PAVILLON_DEPLOYE);
+                }  
+                
+            }
+            break;
+                    
+        case TEST_GIROUS :
+            SendRawId(LECTURE_GIROUETTE) ;
+            wait_ms(5); 
+            canProcessRx();
+            lcd.SetBackColor(LCD_COLOR_WHITE);
+            lcd.SetTextColor(LCD_COLOR_BLACK);
+            lcd.Clear (LCD_COLOR_WHITE);
+            lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"Test Girouette", LEFT_MODE);
+            sprintf(toto,"%hd",val_girou);
+            lcd.DisplayStringAt(0, LINE(10), (unsigned char *)"Valeur Girou :", LEFT_MODE);
+            lcd.DisplayStringAt(0, LINE(11), (unsigned char *)toto, LEFT_MODE);
+            RETOUR.Draw(0xFFFF0000,0);
+                if(RETOUR.Touched()) {
+                    while (RETOUR.Touched());
+                    strat_etat_s=DEMO2;
+                } 
+            wait_ms(200);
+            break ;
+            
         case TEST_ASSERVE:
             lcd.SetBackColor(LCD_COLOR_WHITE);
             lcd.SetTextColor(LCD_COLOR_BLACK);
@@ -231,20 +341,26 @@
             TEST_B.Draw(BLEU, BLANC);                
             TEST_C.Draw(BLEU, BLANC) ;
                 
-            while(strat_etat_s==TEST_ASSERVE) {
-                if(RETOUR.Touched()) {
+            while(strat_etat_s==TEST_ASSERVE) 
+            {
+                if(RETOUR.Touched()) 
+                {
                     while (RETOUR.Touched());
                     strat_etat_s=DEMO2;
-                } else if(TEST_A.Touched()) {
+                } 
+                else if(TEST_A.Touched()) 
+                {
                     while (TEST_A.Touched());
-                    GoToPosition(2000,0,0,0);
+                    GoStraight(2000,0,0,0);
                     TEST_A.Draw(BLEU, BLANC);  
-                }  else if(TEST_B.Touched()) {
+                }  
+                else if(TEST_B.Touched()) 
+                {
                     while (TEST_B.Touched());
-                    Rotate(3599);
+                    Rotate(3600*5);
                     TEST_B.Draw(BLEU, BLANC);
                 }  
-                  else if(TEST_C.Touched()) 
+                else if(TEST_C.Touched()) 
                 {
                     while (TEST_C.Touched());
                     BendRadius(1000,900,1,0);
@@ -868,18 +984,21 @@
                             break ;
                         case 217 :
                             choix_bras = 21 ;
+                            GoStraight(-40, 0, 0, 0);
                             SendMsgCan(BRAS_AT, &choix_bras,sizeof(choix_bras));
                             waitingAckFrom = 0;
                             waitingAckID =0;
                             break ;
                         case 200 :
                             choix_bras = 20 ;
+                            GoStraight(-50, 0, 0, 0);
                             SendMsgCan(BRAS_AT, &choix_bras,sizeof(choix_bras));
                             waitingAckFrom = 0;
                             waitingAckID =0;
                             break ;
                         case 10 :
                             choix_bras = 10 ;
+                            GoStraight(-45, 0, 0, 0);
                             SendMsgCan(BRAS_AT, &choix_bras,sizeof(choix_bras));
                             waitingAckFrom = 0;
                             waitingAckID =0;
@@ -1249,49 +1368,47 @@
                 lcd.DisplayStringAt(100, LINE(8), (unsigned char *)"Score :", LEFT_MODE);
                 lcd.SetTextColor(LCD_COLOR_RED);
                 lcd.DrawRect(0,400,400,320);
-                lcd.DisplayStringAt(100, LINE(22), (unsigned char *)"Timer :", LEFT_MODE);
+                lcd.DisplayStringAt(100, LINE(28), (unsigned char *)"Timer :", LEFT_MODE);
                 unique = 1;
             }
-            int x_offsetc = -145*cos(theta_robot*M_PI/1800) - 0*sin(theta_robot*M_PI*1800);
-            int y_offsetc = -145*sin(theta_robot*M_PI/1800) + 0*cos(theta_robot*M_PI*1800);
+            
             
             lcd.SetTextColor(LCD_COLOR_BLACK);
-            lcd.DisplayStringAt(100, LINE(14), (unsigned char *)"x_robot :", LEFT_MODE);
-            sprintf(toto,"%04d",(short)(x_robot+x_offsetc));
-            lcd.DisplayStringAt(250, LINE(14), (unsigned char *)toto, LEFT_MODE);
             
-            lcd.DisplayStringAt(100, LINE(15), (unsigned char *)"y_robot :", LEFT_MODE);
-            sprintf(toto,"%04d",(short)(y_robot+y_offsetc));
+            lcd.DisplayStringAt(100, LINE(15), (unsigned char *)"girou :", LEFT_MODE);
+            sprintf(toto,"%02d",val_girou);
             lcd.DisplayStringAt(250, LINE(15), (unsigned char *)toto, LEFT_MODE);
             
-            lcd.DisplayStringAt(100, LINE(16), (unsigned char *)"t_robot :", LEFT_MODE);
-            sprintf(toto,"%04d",theta_robot);
+            lcd.DisplayStringAt(100, LINE(16), (unsigned char *)"debug :", LEFT_MODE);
+            sprintf(toto,"%02d",debug_bon_port);
             lcd.DisplayStringAt(250, LINE(16), (unsigned char *)toto, LEFT_MODE);
             
-            lcd.DisplayStringAt(100, LINE(17), (unsigned char *)"g_vert :", LEFT_MODE);
-            sprintf(toto,"%04d",gobelet_vert);
-            lcd.DisplayStringAt(250, LINE(17), (unsigned char *)toto, LEFT_MODE);
             
-            lcd.DisplayStringAt(100, LINE(18), (unsigned char *)"g_rouge :", LEFT_MODE);
+            lcd.DisplayStringAt(100, LINE(10), (unsigned char *)"g_vert :", LEFT_MODE);
+            sprintf(toto,"%04d",gobelet_vert);
+            lcd.DisplayStringAt(250, LINE(10), (unsigned char *)toto, LEFT_MODE);
+            
+            lcd.DisplayStringAt(100, LINE(11), (unsigned char *)"g_rouge :", LEFT_MODE);
             sprintf(toto,"%04d",gobelet_rouge);
-            lcd.DisplayStringAt(250, LINE(18), (unsigned char *)toto, LEFT_MODE);
+            lcd.DisplayStringAt(250, LINE(11), (unsigned char *)toto, LEFT_MODE);
             
-            lcd.DisplayStringAt(100, LINE(19), (unsigned char *)"g_port :", LEFT_MODE);
+            lcd.DisplayStringAt(100, LINE(12), (unsigned char *)"g_port :", LEFT_MODE);
             sprintf(toto,"%04d",gobelet_port);
-            lcd.DisplayStringAt(250, LINE(19), (unsigned char *)toto, LEFT_MODE);
+            lcd.DisplayStringAt(250, LINE(12), (unsigned char *)toto, LEFT_MODE);
             
+    
             actual_time = gameTimer.read();
-            if(actual_time != old_time)
-            {
+//            if(actual_time != old_time)
+//            {
                 lcd.SetTextColor(LCD_COLOR_BLUE);
                 sprintf(toto,"%hd",score_final);
                 lcd.DisplayStringAt(250, LINE(8), (unsigned char *)toto, LEFT_MODE);
                 
                 lcd.SetTextColor(LCD_COLOR_RED);          
                 sprintf(toto,"%hd",actual_time);
-                lcd.DisplayStringAt(250, LINE(22), (unsigned char *)toto, LEFT_MODE);
-                old_time = actual_time;
-            }
+                lcd.DisplayStringAt(250, LINE(28), (unsigned char *)toto, LEFT_MODE);
+//                old_time = actual_time;
+//            }
             break;
     
         
--- a/Strategie/Strategie.cpp	Mon May 31 13:36:03 2021 +0000
+++ b/Strategie/Strategie.cpp	Sat Jul 17 11:07:17 2021 +0000
@@ -14,7 +14,7 @@
 
 Ticker chrono;
 Timeout AffTime;
-Timer timer;
+Ticker timer;
 Timer cartesCheker;//Le timer pour le timeout de la vérification des cartes
 Timer gameTimer;
 Timer debugetatTimer;
@@ -25,6 +25,7 @@
 unsigned char screenChecktry = 0;
 unsigned char test[32] = {32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32};
 
+signed char asser_stop_direction=0;
 char counter = 0;
 char check;
 char Jack = 1;
@@ -40,6 +41,8 @@
 unsigned short flag_check_carte = 0, flag_strat = 0, flag_timer;
 int flagReceptionTelemetres = 0, flagNonRepriseErrorMot = 0;
 
+int Flag_Bras_Re = 0, Flag_Manche_Bas = 0, Flag_Manche_Moy = 0, Flag_pavillon = 0, Flag_bon_port = 0; //Flag utilisé pour compter le score des bras / manches / pavillon
+unsigned short Flag_num_bras;
 
 signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
 
@@ -49,7 +52,7 @@
 //unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
 int flagSendCan=1;
 unsigned char Cote = 0; //0 -> JAUNE | 1 -> VIOLET
-unsigned char Hauteur = 0; 
+unsigned char Hauteur = 0; //Robot du haut -> 1, du bas -> 0
 unsigned short angleRecalage = 0;
 unsigned char checkCurrent = 0;
 unsigned char countAliveCard = 0;
@@ -63,6 +66,9 @@
 unsigned char ingnorBaliseOnce = 0;//une fois détecté réinitialise
 unsigned char ingnorBalise = 0;//0:balise ignore 1:on ecoute la balise
 short direction;
+char val_girou ;
+
+unsigned char debug_bon_port = 0;
 
 unsigned char ingnorInversionOnce = 0;//Pour ignorer l'inversion des instruction une fois
 
@@ -138,7 +144,23 @@
     }
 }
 
+void wait_ISR(void)
+{
+    actual_instruction = instruction.nextLineOK;
+    gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
+    timer.detach();
+}
 
+void girouette_ISR(void)
+{
+    actual_instruction = instruction.nextLineOK;
+    gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;   
+}
+
+void isr_end_danger(void)
+{
+    balise_end_danger(&instruction,&dodgeq,target_x_robot,target_y_robot,target_theta_robot, theta_robot,x_robot,y_robot);
+}
 
 /****************************************************************************************/
 /* FUNCTION NAME: Strategie                                                             */
@@ -151,10 +173,21 @@
     signed char localData1 = 0;
     signed short localData2 = 0;
     unsigned short localData3 = 0;
-    //signed short localData4 = 0;
+    unsigned short localData4 = 0;
     unsigned char localData5 = 0;
+    int unique = 0 ;
 
-
+    if(gameTimer.read_ms() >= 95000)
+    {
+        int unique = 0 ;
+        if (unique == 0)
+        {
+            SendRawId(PAVILLON_DEPLOYE) ;
+            Flag_pavillon=1;
+            unique = 1;
+        }
+    }
+    
     if(gameTimer.read_ms() >= 99000) {//Fin du match (On autorise 2s pour déposer des éléments
         gameTimer.stop();
         gameTimer.reset();
@@ -366,30 +399,82 @@
                         break;
 
                     case GOTOPOS:
-                        localData1 = -1;
-
-                        if(InversStrat == 1 && ingnorInversionOnce == 0) {
-                            localData2 = -instruction.arg3;
-                            localData3 = 3000 - instruction.arg2;//Inversion du Y
-                        } else {
-                            localData3 = instruction.arg2;
-                            localData2 = instruction.arg3;
+                        if(Hauteur == 0)
+                        {
+                            localData1 = -1;
+    
+                            if(InversStrat == 1 && ingnorInversionOnce == 0) {
+                                localData2 = -instruction.arg3;
+                                localData3 = 3000 - instruction.arg2;//Inversion du Y
+                            } else {
+                                localData3 = instruction.arg2;
+                                localData2 = instruction.arg3;
+                            }
+    
+                            GoToPosition(instruction.arg1,localData3,localData2,localData1);
+                            waitingAckID = ASSERVISSEMENT_XYT;
+                            waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+    
+                            while(waitingAckID !=0 && waitingAckFrom !=0)
+                                canProcessRx();
+                            waitingAckID_FIN=ASSERVISSEMENT_XYT;
+                            waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
+                            while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
+                                canProcessRx();
+                            etat_pos=FIN_POS;
                         }
-
-                        GoToPosition(instruction.arg1,localData3,localData2,localData1);
-                        waitingAckID = ASSERVISSEMENT_XYT;
-                        waitingAckFrom = ACKNOWLEDGE_MOTEUR;
-
-                        while(waitingAckID !=0 && waitingAckFrom !=0)
-                            canProcessRx();
-                        waitingAckID_FIN=ASSERVISSEMENT_XYT;
-                        waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
-                        while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
-                            canProcessRx();
-                        etat_pos=FIN_POS;
+                        else if(Hauteur == 1)
+                        {
+                            localData1 = 1;
+    
+                            if(InversStrat == 1 && ingnorInversionOnce == 0) {
+                                localData2 = -instruction.arg3;
+                                localData3 = 3000 - 600;//Inversion du Y
+                            } else {
+                                localData3 = 600;
+                                localData2 = instruction.arg3;
+                            }
+    
+                            GoToPosition(instruction.arg1,localData3,localData2,localData1);
+                            waitingAckID = ASSERVISSEMENT_XYT;
+                            waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+    
+                            while(waitingAckID !=0 && waitingAckFrom !=0)
+                                canProcessRx();
+                            waitingAckID_FIN=ASSERVISSEMENT_XYT;
+                            waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
+                            while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
+                                canProcessRx();
+                                
+                            localData1 = -1;
+    
+                            if(InversStrat == 1 && ingnorInversionOnce == 0) {
+                                localData2 = -instruction.arg3;
+                                localData3 = 3000 - instruction.arg2;//Inversion du Y
+                            } else {
+                                localData3 = instruction.arg2;
+                                localData2 = instruction.arg3;
+                            }
+    
+                            GoToPosition(instruction.arg1,localData3,localData2,localData1);
+                            waitingAckID = ASSERVISSEMENT_XYT;
+                            waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+    
+                            while(waitingAckID !=0 && waitingAckFrom !=0)
+                                canProcessRx();
+                            waitingAckID_FIN=ASSERVISSEMENT_XYT;
+                            waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
+                            while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
+                                canProcessRx();
+                            etat_pos=FIN_POS;
+                        }
+                        
                         break;
                     case FIN_POS:
                         actual_instruction = instruction.nextLineOK;
+                        target_x_robot=x_robot;
+                        target_y_robot=y_robot;
+                        target_theta_robot=theta_robot;
                         break;
                 }
             }
@@ -443,13 +528,13 @@
             Traitement de l'instruction, envoie de la trame CAN
             */
             //debug_Instruction(instruction);
-
             actionPrecedente = instruction.order;
-            switch(instruction.order) {
+            switch(instruction.order) 
+            {
                 case MV_COURBURE://C'est un rayon de courbure
-                    Debug_Audio(3,6);
+//                    Debug_Audio(3,6);
                     float alpha=0, theta=0;
-                    unsigned short alph=0;
+                    short alph=0;
                     actionPrecedente = MV_COURBURE;
                     waitingAckID = ASSERVISSEMENT_COURBURE;
                     waitingAckFrom = ACKNOWLEDGE_MOTEUR;
@@ -474,134 +559,58 @@
                     /*if(InversStrat == 1 && ingnorInversionOnce == 0) {
                         localData1 = -localData1;//Inversion de la direction
                     }*/
-
+                    enum E_InstructionDirection directionxyt;
                     BendRadius(instruction.arg1, localData2, localData1, localData5);
+                    alph=localData2;
                     if(localData2>0) {
                         direction=1;
                     } else {
                         direction=-1;
                     }
-                    if(localData2>0)alph=localData2;
-                    else alph=-localData2;
+                    if(localData2>0)
+                    {
+                        directionxyt=FORWARD;
+                        asser_stop_direction=1; 
+                    }
+                    else 
+                    {
+                        directionxyt=BACKWARD;
+                        asser_stop_direction=-1;
+                    }
                     alpha = localData2*M_PI/1800.0f;
                     theta = theta_robot*M_PI/1800.0f;
-                    
-                    if(instruction.direction == LEFT) {  //-------------LEFT
-                        if(alph<450){ // 1 XYT
-                            dodgeq.inst[0].order = MV_XYT;
-                            dodgeq.inst[0].arg1 = x_robot + instruction.arg1*(sin(alpha+theta)-sin(theta));// X
-                            dodgeq.inst[0].arg2 = y_robot + instruction.arg1*(cos(theta)-cos(alpha+theta));// Y
-                            dodgeq.inst[0].arg3 = theta_robot + alph;// T
-                        }
-                        else if(alph<900){
-                            for(int c=0;c<2;c++){ // 2 points de passages
-                                dodgeq.inst[c].order = MV_XYT;
-                                dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha+theta)-sin(theta));// X
-                                dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(theta)-cos(alpha+theta));// Y
-                                dodgeq.inst[c].arg3 = theta_robot + alph;// T
-                                alpha-=alpha/2.0f;
-                                alph-=alph/2;
-                            }
-                        }
-                        else if(alph<1350){
-                            for(int c=0;c<3;c++){ // 3 points de passages
-                                dodgeq.inst[c].order = MV_XYT;
-                                dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha+theta)-sin(theta));// X
-                                dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(theta)-cos(alpha+theta));// Y
-                                dodgeq.inst[c].arg3 = theta_robot + alph;// T
-                                alpha-=alpha/3.0f;
-                                alph-=alph/3;
-                            }
-                        }
-                        else if(alph<1800){
-                            for(int c=0;c<4;c++){ // 4 points de passages
-                                dodgeq.inst[c].order = MV_XYT;
-                                dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha+theta)-sin(theta));// X
-                                dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(theta)-cos(alpha+theta));// Y
-                                dodgeq.inst[c].arg3 = theta_robot + alph;// T
-                                alpha-=alpha/4.0f;
-                                alph-=alph/4;
-                            }
-                        }
-                        else if(alph<2250){
-                            for(int c=0;c<5;c++){ // 5 points de passages
-                                dodgeq.inst[c].order = MV_XYT;
-                                dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha+theta)-sin(theta));// X
-                                dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(theta)-cos(alpha+theta));// Y
-                                dodgeq.inst[c].arg3 = theta_robot + alph;// T
-                                alpha-=alpha/5.0f;
-                                alph-=alph/5;
-                            }
+                    int nbre;
+                    nbre=abs(alph)/450;
+                    for(int c=0;c<nbre+1;c++)
+                    {
+                        dodgeq.inst[c].order = MV_XYT;
+                        dodgeq.inst[c].direction=directionxyt;
+                        if(instruction.direction == LEFT)  //-------------LEFT
+                        {       
+                            target_x_robot = x_robot + instruction.arg1*(sin(theta+alpha)-sin(theta));// X
+                            target_y_robot = y_robot + instruction.arg1*(cos(theta)-cos(theta+alpha));// Y
+                            target_theta_robot = theta_robot + alph;// T
+                            dodgeq.inst[c].arg1 = target_x_robot;// X
+                            dodgeq.inst[c].arg2 = target_y_robot;// Y
+                            dodgeq.inst[c].arg3 = target_theta_robot;// T
                         }
-                        else {
-                            for(int c=0;c<6;c++){ // 6 points de passages
-                                dodgeq.inst[c].order = MV_XYT;
-                                dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha+theta)-sin(theta));// X
-                                dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(theta)-cos(alpha+theta));// Y
-                                dodgeq.inst[c].arg3 = theta_robot + alph;// T
-                                alpha-=alpha/6.0f;
-                                alph-=alph/6;
-                            }
-                        }
-                    } else { //-----------------------------------------RIGHT
-                        if(alph<450){ // 1 XYT
-                            dodgeq.inst[0].order = MV_XYT;
-                            dodgeq.inst[0].arg1 = x_robot + instruction.arg1*(sin(alpha-theta)+sin(theta));// X
-                            dodgeq.inst[0].arg2 = y_robot + instruction.arg1*(cos(alpha-theta)-cos(theta));// Y
-                            dodgeq.inst[0].arg3 = theta_robot - alph;// T
-                        }
-                        else if(alph<900){
-                            for(int c=0;c<2;c++){ // 2 points de passages
-                                dodgeq.inst[c].order = MV_XYT;
-                                dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha-theta)+sin(theta));// X
-                                dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(alpha-theta)-cos(theta));// Y
-                                dodgeq.inst[c].arg3 = theta_robot - alph;// T
-                                alpha-=alpha/2.0f;
-                            }
+                        else
+                        {       
+                            target_x_robot = x_robot + instruction.arg1*(sin(theta)-sin(theta-alpha));// X
+                            target_y_robot = y_robot + instruction.arg1*(cos(theta-alpha)-cos(theta));// Y
+                            target_theta_robot = theta_robot - alph;// T
+                            dodgeq.inst[c].arg1 = target_x_robot;// X
+                            dodgeq.inst[c].arg2 = target_y_robot;// Y
+                            dodgeq.inst[c].arg3 = target_theta_robot;// T
                         }
-                        else if(alph<1350){
-                            for(int c=0;c<3;c++){ // 3 points de passages
-                                dodgeq.inst[c].order = MV_XYT;
-                                dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha-theta)+sin(theta));// X
-                                dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(alpha-theta)-cos(theta));// Y
-                                dodgeq.inst[c].arg3 = theta_robot - alph;// T
-                                alpha-=alpha/3.0f;
-                            }
-                        }
-                        else if(alph<1800){
-                            for(int c=0;c<4;c++){ // 4 points de passages
-                                dodgeq.inst[c].order = MV_XYT;
-                                dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha-theta)+sin(theta));// X
-                                dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(alpha-theta)-cos(theta));// Y
-                                dodgeq.inst[c].arg3 = theta_robot - alph;// T
-                                alpha-=alpha/4.0f;
-                            }
-                        }
-                        else if(alph<2250){
-                            for(int c=0;c<5;c++){ // 5 points de passages
-                                dodgeq.inst[c].order = MV_XYT;
-                                dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha-theta)+sin(theta));// X
-                                dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(alpha-theta)-cos(theta));// Y
-                                dodgeq.inst[c].arg3 = theta_robot - alph;// T
-                                alpha-=alpha/5.0f;
-                            }
-                        }
-                        else {
-                            for(int c=0;c<6;c++){ // 6 points de passages
-                                dodgeq.inst[c].order = MV_XYT;
-                                dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha-theta)+sin(theta));// X
-                                dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(alpha-theta)-cos(theta));// Y
-                                dodgeq.inst[c].arg3 = theta_robot - alph;// T
-                                alpha-=alpha/6.0f;
-                            }
-                        }
+                        alpha-=alpha/((float)nbre);
+                        alph-=alph/(nbre+1);
                     }
                     break;
 
 
 
                 case MV_LINE://Ligne droite
-                    Debug_Audio(3,8);
                     waitingAckID = ASSERVISSEMENT_RECALAGE;
                     waitingAckFrom = ACKNOWLEDGE_MOTEUR;
                     if(instruction.nextActionType == ENCHAINEMENT) {
@@ -616,6 +625,10 @@
                         }
                     }
                     localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1);
+                    if(instruction.direction == FORWARD)
+                        asser_stop_direction=1;
+                    else
+                        asser_stop_direction=-1;
                     GoStraight(localData2, 0, 0, localData5);
 
                     target_x_robot = x_robot + localData2*cos((double)theta_robot*M_PI/1800);
@@ -624,7 +637,6 @@
 
                     break;
                 case MV_TURN: //Rotation sur place
-                    Debug_Audio(3,9);
                     target_x_robot = x_robot;
                     target_y_robot = y_robot;
                     target_theta_robot = theta_robot + localData2;
@@ -650,12 +662,61 @@
                     Rotate(localData2);
 
                     break;
+                    
                 case MV_XYT:
+                
+                if(Flag_bon_port ==1) //XYT pour la rentrée dans le port
+                {
                     Debug_Audio(3,10);
                     if(instruction.direction == BACKWARD) {
                         localData1 = -1;
+                        asser_stop_direction=-1;
                     } else {
                         localData1 = 1;
+                        asser_stop_direction=1;
+                    }
+                    
+                    if(val_girou == 0) //Si c'est Sud on déplace le X
+                    {
+                        localData4 = 1300; 
+                    }
+                    else
+                    {
+                        localData4 = instruction.arg1;    
+                    }
+                    
+                    if(InversStrat == 1 && ingnorInversionOnce == 0) 
+                    {
+                        localData2 = -instruction.arg3;
+                        localData3 = 3000 - instruction.arg2;//Inversion du Y
+                    } 
+                    else 
+                    {
+                        localData3 = instruction.arg2;
+                        localData2 = instruction.arg3;
+                    }
+                    if((instruction.arg1<=0)||(localData3<=0))
+                        GoToPosition(target_x_robot,target_y_robot,target_theta_robot,localData1);
+                    else
+                        GoToPosition(localData4,localData3,localData2,localData1);
+                    waitingAckID = ASSERVISSEMENT_XYT;
+                    waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+
+                    target_x_robot = instruction.arg1;
+                    target_y_robot = localData3;
+                    target_theta_robot = localData2;
+                    
+                    Flag_bon_port = 0;
+                }
+                else //Sinon on effectue XYT normalement selon les instructions
+                {
+                    Debug_Audio(3,10);
+                    if(instruction.direction == BACKWARD) {
+                        localData1 = -1;
+                        asser_stop_direction=-1;
+                    } else {
+                        localData1 = 1;
+                        asser_stop_direction=1;
                     }
 
                     if(InversStrat == 1 && ingnorInversionOnce == 0) {
@@ -665,65 +726,134 @@
                         localData3 = instruction.arg2;
                         localData2 = instruction.arg3;
                     }
-
-                    GoToPosition(instruction.arg1,localData3,localData2,localData1);
+                    if((instruction.arg1<=0)||(localData3<=0))
+                        GoToPosition(target_x_robot,target_y_robot,target_theta_robot,localData1);
+                    else
+                        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;
+                }
+                break;
+                    
                 case MV_RECALAGE:
-                    if(instruction.nextActionType == MECANIQUE) {
-                        instruction.nextActionType = WAIT;
-
-                        waitingAckID = ASSERVISSEMENT_RECALAGE;
-                        waitingAckFrom = ACKNOWLEDGE_MOTEUR;
-
-                        localData2 = (((instruction.direction == FORWARD)?1:-1)*1000);//On indique une distance de 3000 pour etre sur que le robot va ce recaler
-
-                        if(instruction.precision == RECALAGE_Y) {
-                            localData5 = 2;
-                            if(InversStrat == 1 && ingnorInversionOnce == 0) {
-                                localData3 = 3000 - instruction.arg1;//Inversion du Y
-                            } else {
+                //Effectuer un recalage si on a aucun gobelet ou au moins 2
+                /*
+                    if( ((instruction.direction == FORWARD)&&(etat_groupe[0]==5&&etat_groupe[1]==5) || (etat_groupe[0]==5&&etat_groupe[2]==5) || (etat_groupe[1]==5&&etat_groupe[2]==5))  ||
+                        ((instruction.direction == BACKWARD)&&(etat_groupe[3]==5&&etat_groupe[4]==5) || (etat_groupe[3]==5&&etat_groupe[5]==5) || (etat_groupe[4]==5&&etat_groupe[5]==5)) ||
+                        (etat_groupe[0]!=5 && etat_groupe[1]!=5 && etat_groupe[2]!=5 && etat_groupe[3]!=5 && etat_groupe[4]!=5 && etat_groupe[5]!=5))
+                    {
+                */
+                        if(instruction.nextActionType == MECANIQUE) 
+                        {
+                            instruction.nextActionType = WAIT;
+                            waitingAckID = ASSERVISSEMENT_RECALAGE;
+                            waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+                            localData2 = (((instruction.direction == FORWARD)?1:-1)*1000);//On indique une distance de 3000 pour etre sur que le robot va ce recaler
+                            if(instruction.precision == RECALAGE_Y) 
+                            {
+                                localData5 = 2;
+                                if(InversStrat == 1 && ingnorInversionOnce == 0) 
+                                {
+                                    localData3 = 3000 - instruction.arg1;//Inversion du Y
+                                } 
+                                else 
+                                {
+                                    localData3 = instruction.arg1;
+                                }
+                            } 
+                            else 
+                            {
+                                localData5 = 1;
                                 localData3 = instruction.arg1;
                             }
-                        } else {
-                            localData5 = 1;
-                            localData3 = instruction.arg1;
+                            GoStraight(localData2, localData5, localData3, 0);
+                        } 
+                        else 
+                        { //CAPTEUR
+                            SendRawId(DATA_RECALAGE);
+                            waitingAckID = RECEPTION_RECALAGE;
+                            waitingAckFrom = ACKNOWLEDGE_TELEMETRE;
+    
+                            // On attend que les variables soient actualisé
+                            while(!(waitingAckID == 0 && waitingAckFrom == 0))
+                                canProcessRx();
+                            while(!(waitingAckID_FIN==0 && waitingAckFrom_FIN==0))
+                                canProcessRx();
+    
+                            if(instruction.precision == RECALAGE_Y) // ((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))  (theta_robot < 900 && theta_robot > -900)
+                            {    
+                                SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, recalageDistanceY(), theta_robot);
+                            } 
+                            else if(instruction.precision == RECALAGE_X) 
+                            {
+                                SetOdometrie(ODOMETRIE_SMALL_POSITION, recalageDistanceX(), y_robot, theta_robot);
+                            } 
+                            else if(instruction.precision == RECALAGE_T) 
+                            {
+                                SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, y_robot, recalageAngulaireCapteur() );
+                            }
                         }
-                        GoStraight(localData2, localData5, localData3, 0);
-                    } else { //CAPTEUR
-                        SendRawId(DATA_RECALAGE);
-                        waitingAckID = RECEPTION_RECALAGE;
-                        waitingAckFrom = ACKNOWLEDGE_TELEMETRE;
-
-                        // On attend que les variables soient actualisé
-                        while(!(waitingAckID == 0 && waitingAckFrom == 0))
-                            canProcessRx();
-                        while(!(waitingAckID_FIN==0 && waitingAckFrom_FIN==0))
-                            canProcessRx();
-
-                        if(instruction.precision == RECALAGE_Y) {  // ((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))  (theta_robot < 900 && theta_robot > -900)
-                            SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, recalageDistanceY(), theta_robot);
-                        } else if(instruction.precision == RECALAGE_X) {
-                            SetOdometrie(ODOMETRIE_SMALL_POSITION, recalageDistanceX(), y_robot, theta_robot);
-                        } else if(instruction.precision == RECALAGE_T) {
-                            SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, y_robot, recalageAngulaireCapteur() );
+                    /*}
+                    //Ou effectuer une ligne droite jusqu'à la nouvelle valeur de recalage
+                    
+                    else
+                    {
+                        Debug_Audio(3,8);
+                        instruction.nextActionType = WAIT;
+                        waitingAckID = ASSERVISSEMENT_RECALAGE;
+                        waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+                        
+                        //Calcul de la distance à parcourir pour se retrouver au même point que si on avait fait un recalage
+                        localData2 = instruction.arg1;
+                        if(instruction.precision == RECALAGE_Y) 
+                        {
+                            if(localData2 == 2832) localData2 = 2832 - y_robot;     //Gob+ en y
+                            else if(localData2 == 168) localData2 = y_robot - 168;  //Gob- en y
                         }
+                        else if(instruction.precision == RECALAGE_X)
+                        {
+                            if(localData2 == 1832) localData2 = 1832 - x_robot;     //Gob+ en x
+                            else if(localData2 == 168) localData2 = x_robot - 168;  //Gob- en x 
+                        }
+                        localData2 = ((instruction.direction == FORWARD)?1:-1)*localData2;
+                        GoStraight(localData2, 0, 0, 0);//x,x,x,localData5
+                        
+                        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;
 
                 case ACTION:
-                
                     waitingAckID_FIN = 0;
                     waitingAckFrom_FIN = 0;
                     int tempo = 0;
                     waitingAckID= ACK_ACTION;       //On veut un ack de type action
                     waitingAckFrom = ACKNOWLEDGE_HERKULEX; //de la part des herkulex
+                    if(InversStrat==1 && ingnorInversionOnce == 0)
+                    {
+                        if((instruction.arg1 >= 151 && instruction.arg1 <= 154) || (instruction.arg1 >= 163 && instruction.arg1 <= 164))
+                        {
+                            if(instruction.arg2 == 0) instruction.arg2 = 2; 
+                            else if(instruction.arg2 == 2) instruction.arg2 = 0;
+                            else if(instruction.arg2 == 10) instruction.arg2 = 21;
+                            else if(instruction.arg2 == 21) instruction.arg2 = 10;
+                            else if(instruction.arg2 == 3) instruction.arg2 = 5;
+                            else if(instruction.arg2 == 5) instruction.arg2 = 3;
+                            else if(instruction.arg2 == 43) instruction.arg2 = 54;
+                            else if(instruction.arg2 == 54) instruction.arg2 = 43;
+                        }
+                        else if(instruction.arg1 >= 157 && instruction.arg1 <= 159)
+                        {
+                            if(instruction.arg2 == 0) instruction.arg2 = 1; 
+                            else if(instruction.arg2 == 1) instruction.arg2 = 0;
+                        }
+                    }
                     tempo = doAction(instruction.arg1,instruction.arg2,instruction.arg3);
                     //  unsigned char test=(unsigned char) tempo;
                     // SendMsgCan(0x5BD, &test,1);
@@ -751,7 +881,10 @@
                         //AX12_enchainement++;
 
                     }
-                    break;
+                    /*target_x_robot=x_robot;
+                    target_y_robot=y_robot;
+                    target_theta_robot=theta_robot;
+                    */break;
                 default:
                     //Instruction inconnue, on l'ignore
                     break;
@@ -779,18 +912,25 @@
         case ETAT_GAME_WAIT_ACK:
             canProcessRx();
             //SendSpeed(200);//--------------------------------------------------MODE RALENTI !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
-            if(waitingAckID == 0 && waitingAckFrom == 0) {//Les ack ont été reset, c'est bon on continue
+            if(waitingAckID == 0 && waitingAckFrom == 0) //Les ack ont été reset, c'est bon on continue
+            {   
                 //if(true) {
                 cartesCheker.stop();
-                if(instruction.nextActionType == JUMP) {
-                    if(instruction.jumpAction == JUMP_POSITION) {
+                if(instruction.nextActionType == JUMP) 
+                {
+                    if(instruction.jumpAction == JUMP_POSITION) 
+                    {
                         gameEtat = ETAT_GAME_JUMP_POSITION;
-                    } else { //Pour eviter les erreurs, on dit que c'est par défaut un jump time
+                    } 
+                    else 
+                    { //Pour eviter les erreurs, on dit que c'est par défaut un jump time
                         gameEtat = ETAT_GAME_JUMP_TIME;
                         cartesCheker.reset();//On reset le timeOut
                         cartesCheker.start();
                     }
-                } else if(instruction.nextActionType == WAIT) { ///Actualisation des waiting ack afin d'attendre la fin des actions
+                } 
+                else if(instruction.nextActionType == WAIT) ///Actualisation des waiting ack afin d'attendre la fin des actions
+                {   
                     /*wait_ms(200);
                     #ifdef ROBOT_BIG
                         SetOdometrie(ODOMETRIE_BIG_POSITION, x_robot, y_robot, theta_robot);
@@ -827,17 +967,23 @@
                             break;
                         case ACTION:
 
-                            if (modeTelemetre == 0) {
-                                if (telemetreDistance == 0) {
+                            if (modeTelemetre == 0) 
+                            {
+                                if (telemetreDistance == 0) 
+                                {
                                     waitingAckID_FIN = ACK_FIN_ACTION;// ack de type action
                                     waitingAckFrom_FIN = ACKNOWLEDGE_HERKULEX; //de la part des herkulex/actionneurs
-                                } else if(telemetreDistance == 5000) {
+                                } 
+                                else if(telemetreDistance == 5000) 
+                                {
                                     // on est dans le cas ou l'on fait une ligne suivant la distance du telemetre
                                     //waitingAckID_FIN = ASSERVISSEMENT_RECALAGE;
                                     //waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
                                     telemetreDistance = 0;
                                 }
-                            } else { // si on attend la reponse du telemetre
+                            } 
+                            else // si on attend la reponse du telemetre
+                            { 
                                 //modeTelemetre = 1;
                                 waitingAckID_FIN = OBJET_SUR_TABLE;
                                 waitingAckFrom_FIN = 0;
@@ -846,16 +992,23 @@
                         default:
                             break;
                     }
-                } else {
+                } 
+                else 
+                {
                     gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
                     actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
                 }
-            } else if(cartesCheker.read_ms () > 1000) {
+            } 
+            else if(cartesCheker.read_ms () > 1000) 
+            {
                 cartesCheker.stop();
-                if(screenChecktry >=2) {//La carte n'a pas reçus l'information, on passe à l'instruction d'erreur
+                if(screenChecktry >=2) //La carte n'a pas reçus l'information, on passe à l'instruction d'erreur
+                {
                     actual_instruction = instruction.nextLineError;
                     gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
-                } else {
+                } 
+                else 
+                {
                     gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;//On retourne dans l'strat_etat_s d'envois de l'instruction
                 }
             }
@@ -1102,7 +1255,7 @@
 
             case BALISE_DANGER :
                 SendAck(ACKNOWLEDGE_BALISE, BALISE_END_DANGER);
-                balise_danger();
+                balise_danger(FIFO_lecture);
                 break;
 
             case BALISE_STOP:
@@ -1181,6 +1334,13 @@
                 }
 
                 break;
+                
+            case VALEUR_GIROUETTE :
+                val_girou = msgRxBuffer[FIFO_lecture].data[0] ;
+                girouette_ISR();    
+                
+                break ;
+                
                 default:
                 break;
                 /*
@@ -1191,6 +1351,11 @@
                                // waitingAckFrom_FIN=0;
                                 SendRawId(0x40);
                                 break;*/
+            
+                
+                
+                
+                
         }
         FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
     }
@@ -1619,7 +1784,7 @@
             waitingAckID = 0;
             break;
       
-        case 151:
+        case 151: //Bras attraper
             unsigned char argu_at_bras = arg1;
             if(arg1 == 543) argu_at_bras = 66;
             SendMsgCan(BRAS_AT, &argu_at_bras,sizeof(arg1));
@@ -1627,43 +1792,18 @@
             waitingAckID =0;
             break;
             
-        case 152:
+        case 152: //Bras relacher
             unsigned char argu_re_bras = arg1;
             if(arg1 == 543) argu_re_bras = 66;
             SendMsgCan(BRAS_RE, &argu_re_bras,sizeof(arg1));
-            waitingAckFrom = 0;
-            waitingAckID =0;
-            break;
-/*
-        case 153:
-            unsigned char argu_at_2_bras = arg1;
-            SendMsgCan(BRAS_AT_2, &argu_at_2_bras,sizeof(arg1));
-            waitingAckFrom = 0;
-            waitingAckID =0;
-            break;
-      
-        case 154:
-            unsigned char argu_re_2_bras = arg1;
-            SendMsgCan(BRAS_RE_2, &argu_re_2_bras,sizeof(arg1));
+            SendRawId(0x050);
+            Flag_Bras_Re = 1;
+            Flag_num_bras = argu_re_bras;
             waitingAckFrom = 0;
             waitingAckID =0;
             break;
 
-        case 155:
-            unsigned char argu_at_3_bras = arg1;
-            SendMsgCan(BRAS_AT_3, &argu_at_3_bras,sizeof(arg1));
-            waitingAckFrom = 0;
-            waitingAckID =0;
-            break;
-            
-        case 156:
-            unsigned char argu_re_3_bras = arg1;
-            SendMsgCan(BRAS_RE_3, &argu_re_3_bras,sizeof(arg1));
-            waitingAckFrom = 0;
-            waitingAckID =0;
-            break;
-*/
-        case 153:
+        case 153: //Ventouse attraper
             unsigned char argu_at_vent = arg1;
             if(arg1 == 543) argu_at_vent = 66;
             SendMsgCan(VENT_AT, &argu_at_vent,sizeof(arg1));
@@ -1671,7 +1811,7 @@
             waitingAckID =0;
             break;
             
-        case 154:
+        case 154: //Ventouse Relacher
             unsigned char argu_re_vent = arg1;
             if(arg1 == 543) argu_re_vent = 66;
             SendMsgCan(VENT_RE, &argu_re_vent,sizeof(arg1));
@@ -1679,20 +1819,66 @@
             waitingAckID =0;
             break;
             
-        case 157:
+        case 157: //Manche à air bas
             unsigned char argu_manche_bas = arg1;
             SendMsgCan(AUTOMATE_MANCHE_BAS, &argu_manche_bas,sizeof(arg1));
+            Flag_Manche_Bas = 1;
+            Flag_Manche_Moy = 1;
             waitingAckFrom = 0;
             waitingAckID =0;
             break;
             
-        case 158:
+        case 158: //Manche à air moyen
+            unsigned char argu_manche_moy = arg1;
+            SendMsgCan(AUTOMATE_MANCHE_MOY,&argu_manche_moy,sizeof(arg1));
+            Flag_Manche_Bas = 1;
+            Flag_Manche_Moy = 1;
+            waitingAckFrom = 0;
+            waitingAckID =0;
+            break;
+            
+        case 159: //Manche à air haut
             unsigned char argu_manche_haut = arg1;
             SendMsgCan(AUTOMATE_MANCHE_HAUT, &argu_manche_haut,sizeof(arg1));
             waitingAckFrom = 0;
             waitingAckID =0;
             break;
             
+        case 160 : //lecture de la girouette
+            SendRawId(LECTURE_GIROUETTE) ;
+            debug_bon_port = 1;
+            wait_ms(100);
+            break;  
+            
+        case 161 :  //aller dans le bon port de fin de match
+            Flag_bon_port = 1;
+            waitingAckFrom = 0;
+            waitingAckID =0;
+            break ; 
+            
+        case 162 :  //attendre le temps indiqué en seconde
+            debug_bon_port = 2;
+            timer.attach(&wait_ISR, arg1);
+            break ; 
+            
+        case 163 :  //bras prépare à poser
+            unsigned char argu_prepa_bras = arg1;
+            if(arg1 == 543) argu_prepa_bras = 66;
+            SendMsgCan(BRAS_PREPA, &argu_prepa_bras,sizeof(arg1));
+            waitingAckFrom = 0;
+            waitingAckID =0;
+            break ; 
+        
+        case 164 :  //bras pose les gobelets en préparation
+            unsigned char argu_pose_bras = arg1;
+            if(arg1 == 543) argu_pose_bras = 66;
+            SendMsgCan(BRAS_POSE, &argu_pose_bras,sizeof(arg1));
+            Flag_Bras_Re = 1;
+            Flag_num_bras = argu_re_bras;
+            waitingAckFrom = 0;
+            waitingAckID =0;
+            break ; 
+        
         default:
             retour = 0;//L'action n'existe pas, il faut utiliser le CAN
             break;
--- a/Strategie/Strategie.h	Mon May 31 13:36:03 2021 +0000
+++ b/Strategie/Strategie.h	Sat Jul 17 11:07:17 2021 +0000
@@ -26,6 +26,8 @@
     TEST_AUD, 
     TEST_DIVE,
     TEST_ASSERVE,
+    TEST_MANCHES,
+    TEST_GIROUS,
     SELECT_SIDE, 
     SELECT_ROB,
     TACTIQUE, 
@@ -46,7 +48,7 @@
     ETAT_CONFIG, //attente reception du choix du mode( debug ou game)
     ETAT_GAME_INIT,//Mise en mémoire du fichier de stratégie
     ETAT_GAME_WAIT_FOR_JACK,//Attente du retrait du jack
-    ETAT_GAME_START,//Lancement du timer 90s
+    ETAT_GAME_START,//Lancement du timer 100s
     ETAT_GAME_LOAD_NEXT_INSTRUCTION,
     ETAT_GAME_PROCESS_INSTRUCTION,
     ETAT_GAME_WAIT_ACK,
@@ -60,9 +62,21 @@
 extern E_stratGameEtat gameEtat;
 extern T_etat strat_etat_s;
 extern Timer gameTimer;
+
+extern char val_girou ;
+
+extern int Flag_Bras_Re;
+extern int Flag_Manche_Bas;
+extern int Flag_pavillon;
+extern int Flag_Manche_Moy;
+extern int Flag_bon_port;
+extern unsigned short Flag_num_bras;
+extern unsigned char debug_bon_port;
+
 void canProcessRx(void);
 void Strategie(void);
 short recalageAngulaireCapteur(void);
 short recalageDistanceX(void);
 short recalageDistanceY(void);
+void isr_end_danger(void);
 #endif
--- a/main.cpp	Mon May 31 13:36:03 2021 +0000
+++ b/main.cpp	Sat Jul 17 11:07:17 2021 +0000
@@ -65,8 +65,8 @@
     while(true) {
         automate_etat_ihm();
         Strategie();//Boucle dans l'automate principal
-        gestion_Message_CAN();
-        canProcessRx();
+        gestion_Message_CAN();   
+        canProcessRx();  
     }
 }
 void bluetooth_init(void){