Revision 34:6aa4b46b102e, committed 2018-04-26

Comitter:

Sitkah

Date:

Thu Apr 26 20:14:18 2018 +0000

Parent:

33:388aa0bf6af4

Child:

35:742dc6b200b0

Commit message:

Ne pas tenir compte si besoin du code 2017

Changed in this revision

--- a/Globals/constantes.h	Wed Apr 25 12:42:50 2018 +0000
+++ b/Globals/constantes.h	Thu Apr 26 20:14:18 2018 +0000
@@ -20,8 +20,8 @@
 /****
 ** Variable à modifier en fonction du robot
 ***/
-#define ROBOT_BIG//Indique que l'on va compiler pour le gros robot
-//#define ROBOT_SMALL
+//#define ROBOT_BIG//Indique que l'on va compiler pour le gros robot
+#define ROBOT_SMALL
 
 #ifdef ROBOT_BIG
 
@@ -43,9 +43,9 @@
 #else
 
     #define NOMBRE_CARTES           1 //Le nombre de carte présente sur le petit robot
-    #define POSITION_DEBUT_X 200
-    #define POSITION_DEBUT_Y 150
-    #define POSITION_DEBUT_T 900
+    #define POSITION_DEBUT_X 210
+    #define POSITION_DEBUT_Y 285
+    #define POSITION_DEBUT_T 0
     
     #define BALISE_TIMEOUT 2000
     

--- a/Globals/ident_crac.h	Wed Apr 25 12:42:50 2018 +0000
+++ b/Globals/ident_crac.h	Thu Apr 26 20:14:18 2018 +0000
@@ -152,6 +152,7 @@
 #define RECEPTION_RECALAGE 0x215    //Valeur des télémètres 
 #define DATA_RECALAGE 0x216         //Demande de la valeur de tous les télémètres afin de procèder au récalage
 #define LIRE_PANNEAU 0x217
+#define VIBRO 0x218
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
 ///////////////////////////////////////////////////ENVOI DE PARAMETRES//////////////////////////////////////////////////

--- a/Instruction/Instruction.cpp	Wed Apr 25 12:42:50 2018 +0000
+++ b/Instruction/Instruction.cpp	Thu Apr 26 20:14:18 2018 +0000
@@ -41,6 +41,7 @@
         case 'P': return PRECISION;
         case 'X': return RECALAGE_X;
         case 'Y': return RECALAGE_Y;
+        case 'T': return RECALAGE_T;
         default:  return NOPRECISION;
     } 
 }
@@ -52,6 +53,8 @@
         case 'J': return JUMP;
         case 'W': return WAIT;
         case 'E': return ENCHAINEMENT;
+        case 'M': return MECANIQUE;
+        case 'C': return CAPTEUR;
         default:  return NONEXTACTION;
     } 
 }

--- a/Instruction/Instruction.h	Wed Apr 25 12:42:50 2018 +0000
+++ b/Instruction/Instruction.h	Thu Apr 26 20:14:18 2018 +0000
@@ -27,14 +27,17 @@
     NOPRECISION,// N -> Parametre absent
     PRECISION,  // P -> Precision, verifier la position à la fin du mouvement et refaire un XYT si erreur > 1cm
     RECALAGE_X, // X -> Recalage en X, indique un recalage sur l'axe X
-    RECALAGE_Y  // Y -> Recalage en Y, indique un recalage sur l'axe Y
+    RECALAGE_Y,  // Y -> Recalage en Y, indique un recalage sur l'axe Y
+    RECALAGE_T
 };
 enum E_InstructionNextActionType
 {
     NONEXTACTION,    // N -> Parametre absent
     JUMP, 
     WAIT,
-    ENCHAINEMENT
+    ENCHAINEMENT,
+    MECANIQUE,
+    CAPTEUR
 };
 enum E_InstructionNextActionJumpType
 {

--- a/Robots/Strategie_small.cpp	Wed Apr 25 12:42:50 2018 +0000
+++ b/Robots/Strategie_small.cpp	Thu Apr 26 20:14:18 2018 +0000
@@ -37,8 +37,11 @@
 /* FUNCTION NAME: doAction                                                              */
 /* DESCRIPTION  : Effectuer une action specifique                                       */
 /****************************************************************************************/
-unsigned char doAction(unsigned char id, unsigned short cote, short arg2) {
+unsigned char doAction(unsigned char id, unsigned short arg1, short arg2) {
     int retour = 1;
+    CANMessage msgTx=CANMessage();
+    msgTx.format=CANStandard;
+    msgTx.type=CANData;
     switch(id) {
         case 101: //baisser attrape bloc avant
             SendRawId(BAISSER_ATTRAPE_BLOC);   
@@ -68,7 +71,11 @@
             SendRawId(BLOCAGE_BALLE);
             break;
         case 133: //lance le pwm de tir
-            SendRawId(LANCEMENT_MOTEUR_TIR_ON);
+            msgTx.id=LANCEMENT_MOTEUR_TIR_ON;
+            
+            msgTx.len=1;
+            msgTx.data[0]=arg1;
+            can2.write(msgTx);
             break;
         case 134: ///Arrete le pwm de tir
             SendRawId(LANCEMENT_MOTEUR_TIR_OFF);
@@ -85,6 +92,10 @@
         case 138://position aiguilleur au centre
             SendRawId(TRI_BALLE);
             break;
+        case 139:
+            SendRawId(VIBRO);
+            break;
+        
             
             
         case 200 :
@@ -113,24 +124,24 @@
             setAsservissementEtat(1);
         break; 
         
-        /*case 22://Changer la vitesse du robot
-            SendSpeed(speed,(unsigned short)angle);
+        case 22://Changer la vitesse du robot
+            SendSpeed(arg1,(unsigned short)arg2);
             wait_us(200);
             waitingAckFrom = 0;
             waitingAckID = 0;
         break;
         
         case 19: // CHANGER LA VITESSE + DECELERATION 
-            SendSpeedDecel(speed,(unsigned short) angle);
+            SendSpeedDecel(arg1,(unsigned short) arg2);
             wait_us(200);
             waitingAckFrom = 0;
             waitingAckID =0;
         break;
         
         case 30://Action tempo
-            wait_ms(speed);
+            wait_ms(arg1);
         break;
-        */
+        
         default:
             retour = 0;//L'action n'existe pas, il faut utiliser le CAN
         

--- a/Strategie/Strategie.cpp	Wed Apr 25 12:42:50 2018 +0000
+++ b/Strategie/Strategie.cpp	Thu Apr 26 20:14:18 2018 +0000
@@ -6,9 +6,9 @@
 
 
 #define M_PI 3.14159265358979323846
-#define BLEU 0xFF1467E5
+#define VERT 0xFF1467E5
 #define ROUGE 0xFFFF0000
-#define VERT 0xFF00FF00
+#define BLEU 0xFF00FF00
 #define JAUNE 0xFFFEFE00
 #define BLANC 0xFF000000
 #define ORANGE 0xFFFFA500
@@ -42,7 +42,8 @@
     "End_loop",
 };
 
-
+int waitingAckID_FIN;
+int waitingAckFrom_FIN;
 
 Ticker ticker;
 TS_DISCO_F469NI ts;
@@ -78,7 +79,7 @@
 signed short start_move_x,start_move_y,start_move_theta;//La position du robot lors du début d'un mouvement, utilisé pour reprendre le mouvement apres stop balise
 //unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
 int flagSendCan=1;
-unsigned char Cote = 0; //0 -> bleu | 1 -> jaune
+unsigned char Cote = 0; //0 -> VERT | 1 -> jaune
 
 unsigned char checkCurrent = 0;
 unsigned char countAliveCard = 0; 
@@ -106,8 +107,8 @@
 
 
 /////////////////DEFINITION DES BOUTONS////////////////////
-    Button COTE_BLEU(0, 25, 400, 300, "Bleu");
-    Button COTE_JAUNE(0, 350, 400, 300, "Jaune");
+    Button COTE_VERT(0, 25, 400, 300, "VERT");
+    Button COTE_ORANGE(0, 350, 400, 300, "ORANGE");
     Button RETOUR  (0, 680, 400, 110, "--Precedent--");
     Button LANCER  (0, 200, 400, 200, "--LANCER--");
     Button CHECK (0, 420, 400, 200, "Valider");
@@ -137,8 +138,8 @@
     Button SUIVANT(0,380,200,100,"Suivant");
     Button COLOR_ORANGE (0, 230, 190, 110,"");
     Button COLOR_JAUNE (210, 230, 190, 110,"");
-    Button COLOR_BLEU (0, 350, 190, 110,"");
-    Button COLOR_VERT (210, 350, 190, 110,"");
+    Button COLOR_VERT (0, 350, 190, 110,"");
+    Button COLOR_BLEU (210, 350, 190, 110,"");
     Button COLOR_NOIR (105, 470, 190, 110,"");
     ////////////////////////////////////////////////////////////
 
@@ -153,9 +154,10 @@
 void effacer_segment(long couleur);
 
 unsigned short telemetreDistance=0;
-unsigned short telemetreDistance1=0;
-unsigned short telemetreDistance2=0;
-unsigned short telemetreDistance3=0;
+unsigned short telemetreDistance_avant_gauche=0;
+unsigned short telemetreDistance_avant_droite=0;
+unsigned short telemetreDistance_arriere_gauche=0;
+unsigned short telemetreDistance_arriere_droite=0;
 
 #ifdef ROBOT_BIG
 
@@ -292,7 +294,7 @@
     }
     strcpy(tableau_aff[9],tableau_etat[conv]);
     for(i=0;i<10;i++){
-        lcd.SetBackColor(BLEU);
+        lcd.SetBackColor(VERT);
         lcd.DisplayStringAt(0, LINE(20+i), (uint8_t *)tableau_aff[i], LEFT_MODE);
     }  
     /*while(!ack_bluetooth){
@@ -397,12 +399,12 @@
         case DEMO :                                  
             lcd.Clear(LCD_COLOR_WHITE);
             RETOUR.Draw(0xFFFF0000, 0);
-            TEST_HERKULEX.Draw(BLEU, 0);
-            TEST_LASER.Draw(BLEU, 0);
-            TEST_COULEURS.Draw(BLEU, 0);
-            TEST_TIR_BALLE.Draw(BLEU, 0);
-            TEST_IMMEUBLE.Draw(BLEU,0);
-            TEST_TRIEUR.Draw(BLEU,0);
+            TEST_HERKULEX.Draw(VERT, 0);
+            TEST_LASER.Draw(VERT, 0);
+            TEST_COULEURS.Draw(VERT, 0);
+            TEST_TIR_BALLE.Draw(VERT, 0);
+            TEST_IMMEUBLE.Draw(VERT,0);
+            TEST_TRIEUR.Draw(VERT,0);
             if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config                                     //
                 InversStrat = 0;//Pas d'inversion de la couleur                                 // A changer , discussion avec l'ihm
             }
@@ -553,8 +555,8 @@
                         COLOR_NOIR.Draw(NOIR,1);
                         COLOR_ORANGE.Draw(ORANGE,0);
                         COLOR_JAUNE.Draw(JAUNE,0);
-                        COLOR_BLEU.Draw(BLEU,0);
                         COLOR_VERT.Draw(VERT,0);
+                        COLOR_BLEU.Draw(VERT,0);
                         
                         lcd.SetBackColor(LCD_COLOR_WHITE);
                         lcd.SetTextColor(NOIR);
@@ -809,15 +811,15 @@
             lcd.SetTextColor(LCD_COLOR_BLACK);
              
             lcd.DisplayStringAt(70, LINE(0), (uint8_t *)"Choisir le cote", LEFT_MODE);
-            COTE_BLEU.Draw(BLEU, 0);
-            COTE_JAUNE.Draw(JAUNE, 0);
+            COTE_VERT.Draw(VERT, 0);
+            COTE_ORANGE.Draw(ORANGE, 0);
             RETOUR.Draw(LCD_COLOR_RED, 0);
             
                 
             while (etat == SELECT_SIDE) 
             {
                 canProcessRx();
-                if(COTE_BLEU.Touched()) 
+                if(COTE_VERT.Touched()) 
                 {
                     liaison_Tx.envoyer(0x30,3,"123");
                     Cote = 0x0;
@@ -830,11 +832,11 @@
                     trame_Tx.id=CHOICE_COLOR;
                     trame_Tx.data[0]=Cote;
                     can2.write(trame_Tx);
-                    while(COTE_BLEU.Touched());
+                    while(COTE_VERT.Touched());
                     
                 }
                 
-                if(COTE_JAUNE.Touched())
+                if(COTE_ORANGE.Touched())
                 {
                     Cote = 0x1;
                     InversStrat= Cote;
@@ -846,7 +848,7 @@
                     trame_Tx.id=CHOICE_COLOR;
                     trame_Tx.data[0]=Cote;
                     can2.write(trame_Tx);
-                    while(COTE_JAUNE.Touched());
+                    while(COTE_ORANGE.Touched());
                 }
                 
                 if(RETOUR.Touched())
@@ -860,16 +862,16 @@
                 
         case TACTIQUE :
             if (Cote == 0){
-                lcd.Clear(BLEU);
-                lcd.SetBackColor(BLEU);
+                lcd.Clear(VERT);
+                lcd.SetBackColor(VERT);
                 }
             else if (Cote == 1){
                 lcd.Clear(JAUNE);
                 lcd.SetBackColor(JAUNE);
                 }
             else {
-                lcd.Clear(BLEU);
-                lcd.SetBackColor(BLEU);
+                lcd.Clear(VERT);
+                lcd.SetBackColor(VERT);
                 }
             
             lcd.SetTextColor(LCD_COLOR_BLACK); 
@@ -927,16 +929,16 @@
             lcd.SetTextColor(LCD_COLOR_BLACK);
              
             if (Cote == 0){
-                lcd.Clear(BLEU);
-                lcd.SetBackColor(BLEU);
+                lcd.Clear(VERT);
+                lcd.SetBackColor(VERT);
                 }
             else if (Cote == 1){
                 lcd.Clear(JAUNE);
                 lcd.SetBackColor(JAUNE);
                 }
             else {
-                lcd.Clear(BLEU);
-                lcd.SetBackColor(BLEU);
+                lcd.Clear(VERT);
+                lcd.SetBackColor(VERT);
             }
             canProcessRx();
             lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"En attente du Jack", CENTER_MODE); 
@@ -951,14 +953,24 @@
             lcd.SetTextColor(LCD_COLOR_BLACK);
             cpt=int(cptf);
             if(cpt != cpt1){
-                lcd.Clear(BLEU);
-                affichage_compteur(90-cpt);
+                lcd.Clear(VERT);
+                affichage_compteur(100-cpt);
             }
             cpt1=cpt;
             flag_timer=0;
 
-            affichage_debug(gameEtat);
+            //affichage_debug(gameEtat);
+            lcd.SetBackColor(LCD_COLOR_WHITE);
+            char position_x[10]="toto";
+            char position_y[10]="toto";
+            char position_theta[10]="toto";
+            sprintf(position_x,"%d ",x_robot);
+            sprintf(position_y,"%d",y_robot);
+            sprintf(position_theta,"%d ",theta_robot);
             
+            lcd.DisplayStringAt(120, LINE(18), (uint8_t *)position_x, LEFT_MODE);
+            lcd.DisplayStringAt(120, LINE(20), (uint8_t *)position_y, LEFT_MODE);
+            lcd.DisplayStringAt(120, LINE(22), (uint8_t *)position_theta, LEFT_MODE);
             
             break;
             
@@ -990,7 +1002,7 @@
     //signed short localData4 = 0;
     unsigned char localData5 = 0;
     
-    if(gameTimer.read_ms() >= 89000) {//Fin du match (On autorise 2s pour déposer des éléments
+    if(gameTimer.read_ms() >= 99000) {//Fin du match (On autorise 2s pour déposer des éléments
         gameTimer.stop();
         gameTimer.reset();
         gameEtat = ETAT_END;//Fin du temps
@@ -1135,12 +1147,12 @@
             gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
             
             if (ModeDemo == 0){
-                chronoEnd.attach(&chronometre_ISR,90);//On lance le chrono de 90s
+                chronoEnd.attach(&chronometre_ISR,100);//On lance le chrono de 90s
                 gameTimer.start();
             } 
             gameTimer.reset();
             jack.fall(NULL);//On désactive l'interruption du jack
-            SendRawId(GLOBAL_START);  
+            //SendRawId(GLOBAL_START);  
             Jack=0;                                          //à envoyer sur le CAN et en direct pour l'automate de l'ihm ou sur CANV
             //tactile_printf("Start");//Pas vraiment utile mais bon
         break;
@@ -1271,9 +1283,10 @@
                         }
                     }
                     #endif  
-                    Rotate(localData2);
                     waitingAckID = ASSERVISSEMENT_ROTATION;
                     waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+                    Rotate(localData2);
+                    
                       
                 break;
                 case MV_XYT:
@@ -1310,25 +1323,242 @@
                     
                 break;
                 case MV_RECALAGE:
-                    waitingAckID = ASSERVISSEMENT_RECALAGE;
-                    waitingAckFrom = ACKNOWLEDGE_MOTEUR;
-                    instruction.nextActionType = WAIT;
-                    localData2 = (((instruction.direction == FORWARD)?1:-1)*3000);//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
+                    if(instruction.nextActionType == MECANIQUE)
+                    {
+                        instruction.nextActionType = WAIT;
+                        
+                        waitingAckID = ASSERVISSEMENT_RECALAGE;
+                        waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+                        
+                        localData2 = (((instruction.direction == FORWARD)?1:-1)*3000);//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);
                     }
-                    
-                    GoStraight(localData2, localData5, localData3, 0);
+                    else //CAPTEUR
+                    {
+                        unsigned char nombresDeMesuresAuxTelemtresQuiSontCoherentes         = 0;
+                        unsigned int  moyennageTelemetre                                    = 0;
+                        unsigned short angleAvant = 0;
+                        unsigned short angleArriere = 0;
+                        unsigned short orientationArrondie = 0;
+                        
+                        unsigned short position_avant_gauche=0;
+                        unsigned short position_avant_droite=0;
+                        unsigned short position_arriere_gauche=0;
+                        unsigned short position_arriere_droite=0;
+                        
+                        unsigned short angleRecalage = 0;
+                        unsigned short distanceRecalage = 0;
+                        
+                        SendRawId(DATA_RECALAGE);
+                        waitingAckID = RECEPTION_RECALAGE;
+                        waitingAckFrom = ACKNOWLEDGE_TELEMETRE;
+                        
+                        // On attend que les variables soient actualisé
+                        while(!(waitingAckID == 0 && waitingAckFrom == 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)  
+                        {
+                            telemetreDistance_avant_gauche   = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?3000:0) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?-1:1)*telemetreDistance_avant_gauche;
+                            telemetreDistance_avant_droite   = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?3000:0) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?-1:1)*telemetreDistance_avant_droite;
+                            telemetreDistance_arriere_gauche = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?3000:0) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?-1:1)*telemetreDistance_arriere_gauche;
+                            telemetreDistance_arriere_droite = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?3000:0) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?-1:1)*telemetreDistance_arriere_droite;
+                            
+                            if(telemetreDistance_avant_gauche >= y_robot-instruction.arg1 && telemetreDistance_avant_gauche <= y_robot+instruction.arg1)
+                            {
+                                nombresDeMesuresAuxTelemtresQuiSontCoherentes++;
+                                moyennageTelemetre += telemetreDistance_avant_gauche;
+                            }
+                            if(telemetreDistance_avant_droite >= y_robot-instruction.arg1 && telemetreDistance_avant_droite <= y_robot+instruction.arg1)
+                            {
+                                nombresDeMesuresAuxTelemtresQuiSontCoherentes++;
+                                moyennageTelemetre += telemetreDistance_avant_droite;
+                            }
+                            if(telemetreDistance_arriere_gauche >= y_robot-instruction.arg1 && telemetreDistance_arriere_gauche <= y_robot+instruction.arg1)
+                            {
+                                nombresDeMesuresAuxTelemtresQuiSontCoherentes++;
+                                moyennageTelemetre += telemetreDistance_arriere_gauche;
+                            }
+                            if(telemetreDistance_arriere_droite >= y_robot-instruction.arg1 && telemetreDistance_arriere_droite <= y_robot+instruction.arg1)
+                            {
+                                nombresDeMesuresAuxTelemtresQuiSontCoherentes++;
+                                moyennageTelemetre += telemetreDistance_arriere_droite;
+                            }
+                            
+                            moyennageTelemetre /= nombresDeMesuresAuxTelemtresQuiSontCoherentes;
+                            
+                            if(nombresDeMesuresAuxTelemtresQuiSontCoherentes)
+                                SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, moyennageTelemetre, theta_robot);
+                        }
+                        else if(instruction.precision == RECALAGE_X)
+                        {
+                            telemetreDistance_avant_gauche   = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_avant_gauche;
+                            telemetreDistance_avant_droite   = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_avant_droite;
+                            telemetreDistance_arriere_gauche = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_arriere_gauche;
+                            telemetreDistance_arriere_droite = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_arriere_droite;
+                            
+                            if(telemetreDistance_avant_gauche >= x_robot-instruction.arg1 && telemetreDistance_avant_gauche <= x_robot+instruction.arg1)
+                            {
+                                nombresDeMesuresAuxTelemtresQuiSontCoherentes++;
+                                moyennageTelemetre += telemetreDistance_avant_gauche;
+                            }
+                            if(telemetreDistance_avant_droite >= x_robot-instruction.arg1 && telemetreDistance_avant_droite <= x_robot+instruction.arg1)
+                            {
+                                nombresDeMesuresAuxTelemtresQuiSontCoherentes++;
+                                moyennageTelemetre += telemetreDistance_avant_droite;
+                            }
+                            if(telemetreDistance_arriere_gauche >= x_robot-instruction.arg1 && telemetreDistance_arriere_gauche <= x_robot+instruction.arg1)
+                            {
+                                nombresDeMesuresAuxTelemtresQuiSontCoherentes++;
+                                moyennageTelemetre += telemetreDistance_arriere_gauche;
+                            }
+                            if(telemetreDistance_arriere_droite >= x_robot-instruction.arg1 && telemetreDistance_arriere_droite <= x_robot+instruction.arg1)
+                            {
+                                nombresDeMesuresAuxTelemtresQuiSontCoherentes++;
+                                moyennageTelemetre += telemetreDistance_arriere_droite;
+                            }
+                            
+                            moyennageTelemetre /= nombresDeMesuresAuxTelemtresQuiSontCoherentes;
+                            
+                            if(nombresDeMesuresAuxTelemtresQuiSontCoherentes)
+                                SetOdometrie(ODOMETRIE_SMALL_POSITION, moyennageTelemetre, y_robot, theta_robot);
+                        }
+                        /*else if(instruction.precision == RECALAGE_T)
+                        {
+                            // On arrondie l'angle pour simplifier les conditions
+                            if(theta_robot >= 450 && theta_robot <= 1350)
+                                orientationArrondie = 90; 
+                            else if(theta_robot <= -450 && theta_robot >= -1350)
+                                orientationArrondie = 270;
+                            else if(theta_robot <= 450 && theta_robot >= -450)
+                                orientationArrondie = 0;
+                            else if(theta_robot >= 1350 && theta_robot <= -1350)
+                                orientationArrondie = 180;
+                            
+                            // Calcul de position pour faire la vérification de cohérence
+                            if(orientationArrondie == 90 || orientationArrondie == 270)
+                            {
+                                position_avant_gauche   = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?3000:0) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?-1:1)*telemetreDistance_avant_gauche;
+                                position_avant_droite   = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?3000:0) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?-1:1)*telemetreDistance_avant_droite;
+                                position_arriere_gauche = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?3000:0) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?-1:1)*telemetreDistance_arriere_gauche;
+                                position_arriere_droite = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?3000:0) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?-1:1)*telemetreDistance_arriere_droite;
+                            
+                            }
+                            else if(orientationArrondie == 0 || orientationArrondie == 180)
+                            {
+                                position_avant_gauche   = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_avant_gauche;
+                                position_avant_droite   = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_avant_droite;
+                                position_arriere_gauche = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_arriere_gauche;
+                                position_arriere_droite = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_arriere_droite;
+                            }
+                            
+                            
+                            if(orientationArrondie == 90 || orientationArrondie == 270) // Si il est en axe Y
+                            {
+                                if(position_arriere_droite >= y_robot-instruction.arg1 && position_arriere_droite <= y_robot+instruction.arg1) // Et que les mesures sont cohérentes
+                                {
+                                    if(position_arriere_gauche >= y_robot-instruction.arg1 && position_arriere_gauche <= y_robot+instruction.arg1)
+                                    {
+                                        if(telemetreDistance_arriere_droite > telemetreDistance_arriere_gauche)
+                                            angleArriere = 900 + (1800 * atan( (double)( (telemetreDistance_arriere_droite-telemetreDistance_arriere_gauche) / ESPACE_INTER_TELEMETRE )))/M_PI;                   
+                                        else 
+                                            angleArriere = 900 + (1800 * atan( (double)( (telemetreDistance_arriere_droite-telemetreDistance_arriere_gauche) / ESPACE_INTER_TELEMETRE )))/M_PI;
+                                    }
+                                }
+                            }
+                            else if(orientationArrondie == 0 || orientationArrondie == 180) // Si il est en axe X
+                            {
+                                if(telemetreDistance_arriere_droite >= x_robot-instruction.arg1 && telemetreDistance_arriere_droite <= x_robot+instruction.arg1) // Et que les mesures sont cohérentes
+                                {
+                                    if(telemetreDistance_arriere_gauche >= x_robot-instruction.arg1 && telemetreDistance_arriere_gauche <= x_robot+instruction.arg1)
+                                    {
+                                        if(telemetreDistance_arriere_droite > telemetreDistance_arriere_gauche)
+                                            angleArriere = 900 + (1800 * atan( (double)( (telemetreDistance_arriere_droite-telemetreDistance_arriere_gauche) / ESPACE_INTER_TELEMETRE )))/M_PI;                   
+                                        else 
+                                            angleArriere = 900 + (1800 * atan( (double)( (telemetreDistance_arriere_droite-telemetreDistance_arriere_gauche) / ESPACE_INTER_TELEMETRE )))/M_PI;
+                                    }
+                                }
+                            }
+                            
+                            if(orientationArrondie == 90 || orientationArrondie == 270) // Si il est en axe Y
+                            {
+                                if(position_avant_droite >= y_robot-instruction.arg1 && position_avant_droite <= y_robot+instruction.arg1) // Et que les mesures sont cohérentes
+                                {
+                                    if(position_avant_gauche >= y_robot-instruction.arg1 && position_avant_gauche <= y_robot+instruction.arg1)
+                                    {
+                                        if(telemetreDistance_avant_droite > telemetreDistance_avant_gauche)
+                                            angleAvant = 900 + (1800 * atan( (double)( (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) / ESPACE_INTER_TELEMETRE )))/M_PI;                   
+                                        else 
+                                            angleAvant = 900 + (1800 * atan( (double)( (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) / ESPACE_INTER_TELEMETRE )))/M_PI;
+                                            
+                                        nombresDeMesuresAuxTelemtresQuiSontCoherentes++;
+                                        moyennageTelemetre += angleAvant;
+                                    }
+                                }
+                            }
+                            else if(orientationArrondie == 0 || orientationArrondie == 180) // Si il est en axe X
+                            {
+                                if(telemetreDistance_avant_droite >= x_robot-instruction.arg1 && telemetreDistance_avant_droite <= x_robot+instruction.arg1) // Et que les mesures sont cohérentes
+                                {
+                                    if(telemetreDistance_avant_gauche >= x_robot-instruction.arg1 && telemetreDistance_avant_gauche <= x_robot+instruction.arg1)
+                                    {
+                                        if(telemetreDistance_avant_droite > telemetreDistance_avant_gauche)
+                                            angleAvant = 900 + (1800 * atan( (double)( (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) / ESPACE_INTER_TELEMETRE )))/M_PI;                   
+                                        else 
+                                            angleAvant = 900 + (1800 * atan( (double)( (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) / ESPACE_INTER_TELEMETRE )))/M_PI;
+                                            
+                                        nombresDeMesuresAuxTelemtresQuiSontCoherentes++;
+                                        moyennageTelemetre += angleArriere;
+                                    }
+                                }
+                            }
+                            
+                            angleRecalage = moyennageTelemetre/nombresDeMesuresAuxTelemtresQuiSontCoherentes;
+                            
+                            if(nombresDeMesuresAuxTelemtresQuiSontCoherentes)
+                            {
+                                if(orientationArrondie == 0)
+                                {
+                                    angleRecalage -= 90;
+                                    
+                                    if(telemetreDistance_avant_droite > telemetreDistance_avant_gauche)
+                                        distanceRecalage = *);                  
+                                    else 
+                                        distanceRecalage = 900 + (1800 * atan( (double)( (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) / ESPACE_INTER_TELEMETRE )))/M_PI;     
+                                }
+                                else if(orientationArrondie == 90)
+                                {
+                                    angleRecalage += 0; 
+                                }
+                                else if(orientationArrondie == 180)
+                                {
+                                    angleRecalage += 90; 
+                                }
+                                else if(orientationArrondie == 270)
+                                {
+                                    angleRecalage += 180;
+                                }
+                            }
+                            
+                            
+                            if(nombresDeMesuresAuxTelemtresQuiSontCoherentes)
+                                SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, y_robot, moyennageTelemetre);
+                        }  */                                             
+                    }
                 break;
+                
                 case ACTION:
                     int tempo = 0;
                     waitingAckID= ACK_ACTION;       //On veut un ack de type action
@@ -1336,7 +1566,7 @@
                     tempo = doAction(instruction.arg1,instruction.arg2,instruction.arg3);
                     if(tempo == 1){
                         //L'action est spécifique
-                        if((waitingAckFrom == 0 && waitingAckID == 0) || instruction.nextActionType == ENCHAINEMENT) {
+                        if((waitingAckFrom == 0 && waitingAckID == 0) && instruction.nextActionType == ENCHAINEMENT) {
                             
                             actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
                             gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
@@ -1345,8 +1575,8 @@
                             gameEtat = ETAT_GAME_WAIT_ACK;
                         }
                         #ifdef ROBOT_SMALL
-                            actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
-                            gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
+                            /*actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
+                            gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;*/
                         #endif
                         return;
                      #ifdef ROBOT_SMALL   
@@ -1398,7 +1628,7 @@
             /*
             Attente de l'ack de l'instruction
             */
-            canProcessRx();
+            
             if(waitingAckID == 0 && waitingAckFrom == 0) {//Les ack ont été reset, c'est bon on continue
             //if(true) {
                 cartesCheker.stop();
@@ -1413,45 +1643,45 @@
                     }
                 } 
                 else if(instruction.nextActionType == WAIT) {   ///Actualisation des waiting ack afin d'attendre la fin des actions
-                    gameEtat = ETAT_GAME_WAIT_END_INSTRUCTION;
+                     gameEtat = ETAT_GAME_WAIT_END_INSTRUCTION;
                      switch(instruction.order)
                     {
                         case MV_COURBURE:
-                            waitingAckID = ASSERVISSEMENT_COURBURE;
-                            waitingAckFrom = INSTRUCTION_END_MOTEUR;
+                            waitingAckID_FIN = ASSERVISSEMENT_COURBURE;
+                            waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
                         break;
                         case MV_LINE:
-                            waitingAckID = ASSERVISSEMENT_RECALAGE;
-                            waitingAckFrom = INSTRUCTION_END_MOTEUR;
+                            waitingAckID_FIN = ASSERVISSEMENT_RECALAGE;
+                            waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
                         break;
                         case MV_TURN:
-                            waitingAckID = ASSERVISSEMENT_ROTATION;
-                            waitingAckFrom = INSTRUCTION_END_MOTEUR;
+                            waitingAckID_FIN = ASSERVISSEMENT_ROTATION;
+                            waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
                         break;
                         case MV_XYT:
-                            waitingAckID = ASSERVISSEMENT_XYT;
-                            waitingAckFrom = INSTRUCTION_END_MOTEUR;
+                            waitingAckID_FIN = ASSERVISSEMENT_XYT;
+                            waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
                         break;
                         case MV_RECALAGE:
-                            waitingAckID = ASSERVISSEMENT_RECALAGE;
-                            waitingAckFrom = INSTRUCTION_END_MOTEUR;
+                            waitingAckID_FIN = ASSERVISSEMENT_RECALAGE;
+                            waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
                         break;
                         case ACTION:
                             
                             if (modeTelemetre == 0){
                                 if (telemetreDistance == 0){
-                                    waitingAckID = ACK_FIN_ACTION;// ack de type action  
-                                    waitingAckFrom = ACKNOWLEDGE_HERKULEX; //de la part des herkulex/actionneurs
+                                    waitingAckID_FIN = ACK_FIN_ACTION;// ack de type action  
+                                    waitingAckFrom_FIN = ACKNOWLEDGE_HERKULEX; //de la part des herkulex/actionneurs
                                 }else if(telemetreDistance == 5000){
                                     // on est dans le cas ou l'on fait une ligne suivant la distance du telemetre
-                                    waitingAckID = ASSERVISSEMENT_RECALAGE;
-                                    waitingAckFrom = INSTRUCTION_END_MOTEUR;
+                                    waitingAckID_FIN = ASSERVISSEMENT_RECALAGE;
+                                    waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
                                     telemetreDistance = 0;
                                 }
                             }else{ // si on attend la reponse du telemetre  
                                 //modeTelemetre = 1; 
-                                waitingAckID = OBJET_SUR_TABLE;
-                                waitingAckFrom = 0; 
+                                waitingAckID_FIN = OBJET_SUR_TABLE;
+                                waitingAckFrom_FIN = 0; 
                             }
                         break;
                         default:
@@ -1463,15 +1693,15 @@
                     actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
                 }
             } 
-            else if(cartesCheker.read_ms () > 50){
+            else if(cartesCheker.read_ms () > 500){
                 cartesCheker.stop();
                 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 {
-                    //gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;//On retourne dans l'etat d'envois de l'instruction
-                    gameEtat=ETAT_GAME_WAIT_END_INSTRUCTION;
+                    gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;//On retourne dans l'etat d'envois de l'instruction
+                    //gameEtat=ETAT_GAME_WAIT_END_INSTRUCTION;
                 }
             }
         break;
@@ -1527,10 +1757,20 @@
         break;
         case ETAT_GAME_WAIT_END_INSTRUCTION:
             canProcessRx();
-            if(waitingAckID == 0 && waitingAckFrom ==0) {//On attend que la carte nous indique que l'instruction est terminée
+            if(instruction.order==MV_XYT && (waitingAckID_FIN == 0 && waitingAckFrom_FIN ==0)){
+                if((x_robot<=target_x_robot-2 || x_robot>=target_x_robot+2)||(y_robot<=target_y_robot-2 || y_robot>=target_y_robot+2)||(theta_robot<=target_theta_robot-2 || theta_robot>=target_theta_robot+2)){
+                    gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;
+                }
+                else {
+                    actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
+                    gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante
+                }
+            }      
+            else if(waitingAckID_FIN == 0 && waitingAckFrom_FIN ==0) {//On attend que la carte nous indique que l'instruction est terminée
                 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
                 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante
             }
+            
         break;
         
         
@@ -1671,10 +1911,10 @@
             if (etat == ATT) {
                  
                 lcd.SetTextColor(LCD_COLOR_LIGHTGREEN);
-                lcd.FillRect(0,75,400,150); //carte moteur
+                lcd.FillRect(0,400,400,150);
                 lcd.SetTextColor(LCD_COLOR_BLACK);
                 lcd.SetBackColor(LCD_COLOR_LIGHTGREEN);
-                lcd.DisplayStringAt(80, 135, (uint8_t *)"Carte Moteur", LEFT_MODE);
+                lcd.DisplayStringAt(80, 450, (uint8_t *)"Carte Moteur", LEFT_MODE);
                 }
                 break;
                 
@@ -1682,10 +1922,10 @@
             if (etat == ATT) {
                  
                 lcd.SetTextColor(LCD_COLOR_LIGHTGREEN);
-                lcd.FillRect(0,250,400,150); //carte AX12
+                lcd.FillRect(0,600,400,150); //carte AX12
                 lcd.SetTextColor(LCD_COLOR_BLACK);
                 lcd.SetBackColor(LCD_COLOR_LIGHTGREEN);
-                lcd.DisplayStringAt(110, 310, (uint8_t *)"Balise", LEFT_MODE);
+                lcd.DisplayStringAt(110, 650, (uint8_t *)"Balise", LEFT_MODE);
                 } 
                 break;
 
@@ -1716,9 +1956,10 @@
             /////////////////////////////////////Acknowledges de Reception de la demande d'action////////////////////////////////////////   
             case ACKNOWLEDGE_HERKULEX: 
             case ACKNOWLEDGE_BALISE:    //pas de break donc passe directement dans INSTRUCTION_END_AX12 mais conserve l'ident initial
-            case ACKNOWLEDGE_MOTEUR:
+            
             case ACKNOWLEDGE_TELEMETRE:    
             /////////////////////////////////////////////Acknowledges de la fin d'action/////////////////////////////////////////////////  
+            case ACKNOWLEDGE_MOTEUR:
             case INSTRUCTION_END_BALISE:
             case INSTRUCTION_END_MOTEUR:
             case ACK_FIN_ACTION:
@@ -1727,6 +1968,11 @@
                     waitingAckFrom = 0;
                     waitingAckID = 0;        
                 }
+                if(waitingAckFrom_FIN == msgRxBuffer[FIFO_lecture].id && ((unsigned short)msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8) == waitingAckID_FIN)) {
+                    //SendRawId(waitingAckID);
+                    waitingAckFrom_FIN = 0;
+                    waitingAckID_FIN = 0;        
+                }
                 
             break;
 #ifdef ROBOT_BIG
@@ -1824,33 +2070,34 @@
                 break;
                 
             case RECEPTION_RECALAGE:
-                telemetreDistance=char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[0], msgRxBuffer[FIFO_lecture].data[1]);
-                telemetreDistance1=char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[2], msgRxBuffer[FIFO_lecture].data[3]);
-                telemetreDistance2=char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[4], msgRxBuffer[FIFO_lecture].data[5]);
-                telemetreDistance3=char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[6], msgRxBuffer[FIFO_lecture].data[7]);
-                waitingAckFrom = 0;
-                waitingAckID = 0;
-                if(ModeDemo==1){
-                    sprintf(message,"%04d mm",telemetreDistance);
+                telemetreDistance_arriere_droite = char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[0], msgRxBuffer[FIFO_lecture].data[1]); //
+                telemetreDistance_avant_droite   = char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[2], msgRxBuffer[FIFO_lecture].data[3]);
+                telemetreDistance_arriere_gauche = char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[4], msgRxBuffer[FIFO_lecture].data[5]);
+                telemetreDistance_avant_gauche   = char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[6], msgRxBuffer[FIFO_lecture].data[7]);
+                
+                
+                
+                if(ModeDemo==1)
+                {
+                    sprintf(message,"%04d mm",telemetreDistance_arriere_droite);
                     lcd.SetBackColor(LCD_COLOR_WHITE);
                     lcd.DisplayStringAt(0, LINE(10),(unsigned char *)"LASER ARD : ",LEFT_MODE);
                     lcd.DisplayStringAt(200, LINE(10),(unsigned char *)message, LEFT_MODE);
                     
-                    sprintf(message1,"%04d mm",telemetreDistance1);
+                    sprintf(message1,"%04d mm",telemetreDistance_avant_droite);
                     lcd.SetBackColor(LCD_COLOR_WHITE);
                     lcd.DisplayStringAt(0, LINE(12),(unsigned char *)"LASER AVD : ",LEFT_MODE);
                     lcd.DisplayStringAt(200, LINE(12),(unsigned char *)message1, LEFT_MODE);
                     
-                    sprintf(message2,"%04d mm",telemetreDistance2);
+                    sprintf(message2,"%04d mm",telemetreDistance_arriere_gauche);
                     lcd.SetBackColor(LCD_COLOR_WHITE);
                     lcd.DisplayStringAt(0, LINE(14),(unsigned char *)"LASER ARG : ",LEFT_MODE);
                     lcd.DisplayStringAt(200, LINE(14),(unsigned char *)message2, LEFT_MODE);
                     
-                    sprintf(message3,"%04d mm",telemetreDistance3);
+                    sprintf(message3,"%04d mm",telemetreDistance_avant_gauche);
                     lcd.SetBackColor(LCD_COLOR_WHITE);
                     lcd.DisplayStringAt(0, LINE(16),(unsigned char *)"LASER AVG : ",LEFT_MODE);
-                    lcd.DisplayStringAt(200, LINE(16),(unsigned char *)message3, LEFT_MODE);
-                    
+                    lcd.DisplayStringAt(200, LINE(16),(unsigned char *)message3, LEFT_MODE);                    
                 }
                 break;
             

--- a/main.cpp	Wed Apr 25 12:42:50 2018 +0000
+++ b/main.cpp	Thu Apr 26 20:14:18 2018 +0000
@@ -49,6 +49,7 @@
     if (can2.read(msgRxBuffer[FIFO_ecriture])) {
         //if(msgRxBuffer[FIFO_ecriture].id==RESET_STRAT) mbed_reset();
         /*else*/ FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;
+        canProcessRx();
     }
 }