Programme carte strategie (disco)

Dependencies:   mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac

Revision:
31:833fc481b002
Parent:
30:a1e37af4bbde
Child:
32:1c9ab15c740e
--- a/Strategie/Strategie.cpp	Fri Apr 20 09:16:13 2018 +0000
+++ b/Strategie/Strategie.cpp	Tue Apr 24 18:09:51 2018 +0000
@@ -1,5 +1,6 @@
 #include "global.h"
-
+#include <string.h>
+#include <sstream> 
 //#include "StrategieManager.h"
 
 
@@ -10,6 +11,8 @@
 #define VERT 0xFF00FF00
 #define JAUNE 0xFFFEFE00
 #define BLANC 0xFF000000
+#define ORANGE 0xFFFFA500
+#define NOIR 0xFF000000
 #define DIY_GREY 0xFFDFDFDF 
 
 char tableau_aff[10][50];
@@ -96,7 +99,7 @@
 float cptf; 
 int cpt,cpt1;  
 
-typedef enum {INIT, ATT, CHOIX, DEMO, DEMO_EN_COURS, SELECT_SIDE, TACTIQUE, DETAILS,LECTURE, LAUNCH, AFF_WAIT_JACK, WAIT_JACK, COMPTEUR, FIN} T_etat;
+typedef enum {INIT, ATT, CHOIX, DEMO, TEST_TELEMETRE, TEST_CAPTEURS, TEST_SERVO, TEST_TIR, DEMO_IMMEUBLE, SELECT_SIDE, TACTIQUE, DETAILS,LECTURE, LAUNCH, AFF_WAIT_JACK, WAIT_JACK, COMPTEUR, FIN} T_etat;
 T_etat etat = INIT;
 E_stratGameEtat     gameEtat  = ETAT_CHECK_CARTES;
 E_stratGameEtat     lastEtat  = ETAT_CHECK_CARTES;
@@ -110,10 +113,28 @@
     Button CHECK (0, 420, 400, 200, "Valider");
     Button MATCH (0, 50, 400, 320, "Match");
     Button DEMONSTRATION (0, 400, 400, 320, "Demo");
-    Button TEST_MOTEUR(0, 25, 400, 200, "Demo immeuble");
-    Button TEST_BRAS(0, 250, 400, 200, "Demo Bluetooth");
+    Button TEST_HERKULEX(0, 25, 400, 100, "Test servos");
+    Button TEST_LASER(0, 150, 400, 100, "Test telemetre");
+    Button TEST_COULEURS(0,275,400,100,"Test capteurs");
+    Button TEST_TIR_BALLE(0,400,400,100,"Test Lanceur");
+    Button TEST_IMMEUBLE(0,525,400,100,"Test immeuble");
+    Button TIR_CHATEAU(0, 25, 400, 100, "Tir chateau");
+    Button EPURATION(0, 150, 400, 100, "epuration");
+    Button LANCEUR_ON(0,275,400,100,"allumer le lanceur");
+    Button LANCEUR_OFF(0,400,400,100,"eteindre le lanceur");
+    Button ABAISSE_BLOC(0, 25, 400, 100, "Ramasser blocs");
+    Button RELEVE_BLOC(0, 135, 400, 100, "lacher blocs");
+    Button BRAS_ABEILLE_ON(0,245,400,100,"bras abeille");
+    Button BRAS_ABEILLE_OFF(0,355,400,100,"lever bras interrupt");
+    Button INTERRUPTEUR_ON(0,465,400,100,"baisser bras interrupt");
+    Button INTERRUPTEUR_OFF(0,575,400,100,"baisser bras interrupt");
     Button FORCE_LAUNCH(0, 50, 400, 320, "Force Launch");
     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_NOIR (105, 470, 190, 110,"");
     ////////////////////////////////////////////////////////////
 
 void SendRawId (unsigned short id);
@@ -137,13 +158,13 @@
 unsigned short id_check[NOMBRE_CARTES]= {CHECK_MOTEUR,CHECK_BALISE,CHECK_ACTIONNEURS_AVANT,CHECK_ACTIONNEURS_ARRIERE};
 unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_MOTEUR,ALIVE_BALISE,ALIVE_ACTIONNEURS_AVANT,ALIVE_ACTIONNEURS_ARRIERE};
 
-InterruptIn jack(PA_6); //  entrée analogique en interruption pour le jack
+InterruptIn jack(PG_11); //  entrée analogique en interruption pour le jack
 #else
 
 
 unsigned short id_check[NOMBRE_CARTES]= {CHECK_MOTEUR};
 unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_MOTEUR};
-InterruptIn jack(PA_6); //  entrée analogique en interruption pour le jack
+InterruptIn jack(PG_11); //  entrée analogique en interruption pour le jack
 
 
 #endif
@@ -373,38 +394,64 @@
         
         case DEMO :                                  
             lcd.Clear(LCD_COLOR_WHITE);
-
-            TEST_MOTEUR.Draw(0xFFF0F0F0, 0);
-            TEST_BRAS.Draw(0xFFF0F0F0, 0);
             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);
             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
             }
             while (etat == DEMO)
             {
                 canProcessRx();
-                if(TEST_MOTEUR.Touched())
+                if(TEST_HERKULEX.Touched())
                 {
                     //Strat = 0x10;
-                    while(TEST_MOTEUR.Touched());
-                    TEST_MOTEUR.Draw(0xFFF0F0F0, 0);
-                    etat = DEMO_EN_COURS;
-                    ModeDemo=0;
-                }
-                
-                if(TEST_BRAS.Touched())
-                {
-                    //Strat = 0x11;                        
-                    while(TEST_BRAS.Touched());                 
-                    TEST_MOTEUR.Draw(0xFFF0F0F0, 0);
-                    etat = DEMO_EN_COURS;
+                    while(TEST_HERKULEX.Touched());
+                    trame_Tx.id=CHOICE_COLOR;
+                    trame_Tx.data[0]=2;
+                    can2.write(trame_Tx);
+                    TEST_HERKULEX.Draw(0xFFF0F0F0, 0);
+                    etat = TEST_SERVO;
+                    lcd.Clear(LCD_COLOR_WHITE); 
                     ModeDemo=1;
                 }
                 
+                else if(TEST_LASER.Touched())
+                {
+                    //Strat = 0x11;                        
+                    while(TEST_LASER.Touched());                 
+                    TEST_LASER.Draw(0xFFF0F0F0, 0);
+                    etat = TEST_TELEMETRE;
+                }
+                else if (TEST_COULEURS.Touched()){
+                    while(TEST_COULEURS.Touched());
+                    TEST_LASER.Draw(0xFFF0F0F0, 0);  
+                    etat =TEST_CAPTEURS ;    
+                }
+                else if (TEST_TIR_BALLE.Touched()){
+                    while(TEST_TIR_BALLE.Touched());
+                    TEST_TIR_BALLE.Draw(0xFFF0F0F0, 0);  
+                    etat =TEST_TIR ; 
+                    lcd.Clear(LCD_COLOR_WHITE); 
+                    trame_Tx.id=CHOICE_COLOR;
+                    trame_Tx.data[0]=2;
+                    can2.write(trame_Tx);
+                    ModeDemo=1;  
+                }
+                else if(TEST_IMMEUBLE.Touched()){
+                     while(TEST_IMMEUBLE.Touched());
+                     TEST_IMMEUBLE.Draw(0xFFF0F0F0, 0);  
+                     etat =DEMO_IMMEUBLE;
+                     lcd.Clear(LCD_COLOR_WHITE);    
+                }
                 if(RETOUR.Touched())
                 {
                     etat = CHOIX;
                     while(RETOUR.Touched());
+                   
                 }
                 if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config
                     /*if (Strat< 0x10){  // Si la strat est une strat de match, on desactive le mode demo
@@ -413,7 +460,6 @@
                     else {                                        // sinon, on active le mode demo, utile pour la fin de la demo
                             ModeDemo = 1;
                     }*/
-                    
                     Ack_strat = 1;
                     wait_ms(10);
                     //tactile_printf("Strat %d, Asser desactive",Strat);
@@ -421,62 +467,275 @@
                 //SelectionStrat(Strategie);
             }
             break;
+        case DEMO_IMMEUBLE:
+            int color=0;
+            lcd.SetBackColor(LCD_COLOR_WHITE);
+            lcd.DisplayStringAt(20, LINE(2), (uint8_t *)"Choix du code couleur", LEFT_MODE);
             
-        case DEMO_EN_COURS:
-            lcd.Clear(LCD_COLOR_WHITE);
-            lcd.SetBackColor(LCD_COLOR_WHITE);
-            lcd.DisplayStringAt(20, LINE(10), (uint8_t *)"DEMONSTRATION EN COURS", LEFT_MODE);
+            CANMessage msgTx=CANMessage();
+            msgTx.id=MONTER_IMMEUBLE; // Monter immeuble
+            msgTx.len=3;
+            msgTx.format=CANStandard;
+            msgTx.type=CANData;
             
             
-            while(etat == DEMO_EN_COURS) 
-            {
-                canProcessRx();
-                
-                /*if (Ack_strat == 1)
-                {
-                    if(gameEtat == ETAT_CONFIG) {
-                        gameEtat = ETAT_GAME_INIT;
-                    }              
-                    while (flag_strat != 1)
-                    {
-                      canProcessRx();
-                    }
-                    etat = DEMO;
-                    flag_strat = 0;  
-                }*/
-                if(ModeDemo==0){
-                    CANMessage msgTx=CANMessage();
-                    msgTx.id=0x090; // Monter immeuble
-                    msgTx.len=3;
-                    msgTx.format=CANStandard;
-                    msgTx.type=CANData;
-                    // Rayon sur 2 octets
-                    msgTx.data[0]=4;
-                    msgTx.data[1]=3;
-                    msgTx.data[2]=2;
-                    can2.write(msgTx);
-                    ModeDemo==3;
-                    while(1);
+            while(etat==DEMO_IMMEUBLE){
+                switch(color){
+                    case 0:
+                        
+                        RETOUR.Draw(ROUGE,0);
+                        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);
+                        
+                        lcd.SetBackColor(LCD_COLOR_WHITE);
+                        lcd.SetTextColor(NOIR);
+                        lcd.DisplayStringAt(100, LINE(4), (uint8_t *)"COULEUR 1", LEFT_MODE);
+                        while(color==0){
+                            if(COLOR_ORANGE.Touched()){
+                                while(COLOR_ORANGE.Touched());
+                                COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
+                                msgTx.data[color]=1; 
+                                color++;
+                            }
+                            else if (COLOR_NOIR.Touched()){
+                                while(COLOR_NOIR.Touched());
+                                COLOR_NOIR.Draw(LCD_COLOR_WHITE);
+                                msgTx.data[color]=2; 
+                                color++;
+                            }
+                            else if (COLOR_VERT.Touched()){
+                                while(COLOR_VERT.Touched());
+                                COLOR_VERT.Draw(LCD_COLOR_WHITE);
+                                
+                                msgTx.data[color]=3;
+                                color++;
+                            }
+                            else if (COLOR_JAUNE.Touched()){
+                                while(COLOR_JAUNE.Touched());
+                                COLOR_JAUNE.Draw(LCD_COLOR_WHITE);
+                                msgTx.data[color]=4;
+                                color++;
+                            }
+                            else if (COLOR_BLEU.Touched()){
+                                while(COLOR_BLEU.Touched());
+                                COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
+                                msgTx.data[color]=5;
+                                color++;
+                            }
+                        }
+                        break;
+                        
+                    case 1:
+                        lcd.SetBackColor(LCD_COLOR_WHITE);
+                        lcd.SetTextColor(NOIR);
+                        lcd.DisplayStringAt(100, LINE(4), (uint8_t *)"COULEUR 2", LEFT_MODE);
+                        if(COLOR_ORANGE.Touched()){
+                                while(COLOR_ORANGE.Touched());
+                                COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
+                                msgTx.data[color]=1; 
+                                color++;
+                            }
+                            else if (COLOR_NOIR.Touched()){
+                                while(COLOR_NOIR.Touched());
+                                COLOR_NOIR.Draw(LCD_COLOR_WHITE);
+                                msgTx.data[color]=2; 
+                                color++;
+                            }
+                            else if (COLOR_VERT.Touched()){
+                                while(COLOR_VERT.Touched());
+                                COLOR_VERT.Draw(LCD_COLOR_WHITE);
+                                
+                                msgTx.data[color]=3;
+                                color++;
+                            }
+                            else if (COLOR_JAUNE.Touched()){
+                                while(COLOR_JAUNE.Touched());
+                                COLOR_JAUNE.Draw(LCD_COLOR_WHITE);
+                                msgTx.data[color]=4;
+                                color++;
+                            }
+                            else if (COLOR_BLEU.Touched()){
+                                while(COLOR_BLEU.Touched());
+                                COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
+                                msgTx.data[color]=5;
+                                color++;
+                            }
+                        break;
+                        
+                    case 2:
+                        lcd.SetBackColor(LCD_COLOR_WHITE);
+                        lcd.SetTextColor(NOIR);
+                        lcd.DisplayStringAt(100, LINE(4), (uint8_t *)"COULEUR 3", LEFT_MODE);
+                        if(COLOR_ORANGE.Touched()){
+                                while(COLOR_ORANGE.Touched());
+                                COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
+                                msgTx.data[color]=1; 
+                                color++;
+                            }
+                            else if (COLOR_NOIR.Touched()){
+                                while(COLOR_NOIR.Touched());
+                                COLOR_NOIR.Draw(LCD_COLOR_WHITE);
+                                msgTx.data[color]=2; 
+                                color++;
+                            }
+                            else if (COLOR_VERT.Touched()){
+                                while(COLOR_VERT.Touched());
+                                COLOR_VERT.Draw(LCD_COLOR_WHITE);
+                                
+                                msgTx.data[color]=3;
+                                color++;
+                            }
+                            else if (COLOR_JAUNE.Touched()){
+                                while(COLOR_JAUNE.Touched());
+                                COLOR_JAUNE.Draw(LCD_COLOR_WHITE);
+                                msgTx.data[color]=4;
+                                color++;
+                            }
+                            else if (COLOR_BLEU.Touched()){
+                                while(COLOR_BLEU.Touched());
+                                COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
+                                msgTx.data[color]=5;
+                                color++;
+                            }
+                        break;
+                    case 3:
+                        lcd.Clear(LCD_COLOR_WHITE);
+                        lcd.SetBackColor(LCD_COLOR_WHITE);
+                        lcd.SetTextColor(NOIR);
+                        
+                        lcd.DisplayStringAt(0, LINE(4), (uint8_t *)"Immeuble en construction", LEFT_MODE);
+                        RETOUR.Draw(ROUGE,0);
+                        can2.write(msgTx);
+                        color++;
+                        break;
+                    
+                    case 4:
+                        if(RETOUR.Touched()){
+                            while(RETOUR.Touched());
+                            etat=DEMO;    
+                        }
+                        break;
                 }
-                else if (ModeDemo==1){
-                    CANMessage msgTx=CANMessage();
-                    msgTx.id=DATA_TELEMETRE; // Monter immeuble
-                    msgTx.len=1;
-                    msgTx.format=CANStandard;
-                    msgTx.type=CANData;
-                    // Rayon sur 2 octets
-                    msgTx.data[0]=0;
-                    can2.write(msgTx);
-                    wait(1);
+                if(RETOUR.Touched()){
+                    while(RETOUR.Touched());
                     etat=DEMO;
                 }
+            }
+            break;
+                        
+                        
+                        
+                    
+            
+        case TEST_SERVO:
+            lcd.SetBackColor(LCD_COLOR_WHITE);
+            lcd.DisplayStringAt(20, LINE(2), (uint8_t *)"DEMONSTRATION COURS", LEFT_MODE);
+            ABAISSE_BLOC.Draw(VERT, 0);
+            RELEVE_BLOC.Draw(VERT, 0);
+            BRAS_ABEILLE_ON.Draw(VERT, 0);
+            BRAS_ABEILLE_OFF.Draw(VERT, 0);
+            INTERRUPTEUR_ON.Draw(VERT, 0);
+            INTERRUPTEUR_OFF.Draw(VERT, 0);
+            RETOUR.Draw(0xFFFF0000,0);
+            while(etat==TEST_SERVO){
+                if(RETOUR.Touched()){
+                    while (RETOUR.Touched());
+                    etat=DEMO;
+                }
+                else if(ABAISSE_BLOC.Touched()){
+                    while (ABAISSE_BLOC.Touched());  
+                    SendRawId(BAISSER_ATTRAPE_BLOC);
+                    break;
+                }
+                else if(RELEVE_BLOC.Touched()){
+                    while (RELEVE_BLOC.Touched());  
+                    SendRawId(RELEVER_ATTRAPE_BLOC);
+                    break;
+                } 
+                else if(BRAS_ABEILLE_ON.Touched()){
+                    while (BRAS_ABEILLE_ON.Touched());
+                    SendRawId(BRAS_ABEILLE_UP); 
+                    break;
+                     
+                } 
+                else if(BRAS_ABEILLE_OFF.Touched()){
+                    while (BRAS_ABEILLE_OFF.Touched());  
+                    SendRawId(BRAS_ABEILLE_DOWN);
+                    break; 
+                } 
+                else if(INTERRUPTEUR_ON.Touched()){
+                    while (INTERRUPTEUR_ON.Touched()); 
+                    SendRawId(ALLUMER_PANNEAU_UP); 
+                    break; 
+                }       
+                else if(INTERRUPTEUR_OFF.Touched()){
+                    while (INTERRUPTEUR_OFF.Touched());  
+                    SendRawId(ALLUMER_PANNEAU_DOWN);  
+                    break;
+                }
+            } 
+            break;
+        
+        case TEST_TIR:
+            lcd.SetBackColor(LCD_COLOR_WHITE);
+            lcd.DisplayStringAt(20, LINE(2), (uint8_t *)"DEMONSTRATION COURS", LEFT_MODE);
+            TIR_CHATEAU.Draw(VERT, 0);
+            EPURATION.Draw(VERT, 0);
+            LANCEUR_ON.Draw(VERT, 0);
+            LANCEUR_OFF.Draw(VERT, 0);
+            RETOUR.Draw(ROUGE, 0);
+            while(etat==TEST_TIR){
+                if(TIR_CHATEAU.Touched()){
+                    while (TIR_CHATEAU.Touched());
+                    SendRawId(INCLINAISON_CHATEAU);
+                    break;
+                }
+                else if (EPURATION.Touched()){
+                    while (EPURATION.Touched());
+                    SendRawId(INCLINAISON_EPURATION);
+                    break;
+                }
+                else if(LANCEUR_ON.Touched()){
+                    while (LANCEUR_ON.Touched());
+                    SendRawId(LANCEMENT_MOTEUR_TIR_ON);
+                    break;
+                }
+                else if(LANCEUR_OFF.Touched()){
+                    while (LANCEUR_OFF.Touched());
+                    SendRawId(LANCEMENT_MOTEUR_TIR_OFF);
+                    break;
+                }
+                else if (RETOUR.Touched()){
+                    while (RETOUR.Touched());
+                    etat=DEMO;
+                    
+                }
+            }
+            break;     
                     
                 
-                  
+            
+        case TEST_TELEMETRE:
+            ModeDemo=1;
+            lcd.Clear(LCD_COLOR_WHITE);
+            lcd.SetBackColor(LCD_COLOR_WHITE);
+            lcd.DisplayStringAt(20, LINE(2), (uint8_t *)"DEMONSTRATION COURS", LEFT_MODE);
+            RETOUR.Draw(0xFFFF0000, 0);
+            while(etat==TEST_TELEMETRE){
+                SendRawId(DATA_RECALAGE);
+                wait(0.1);
+                canProcessRx();
+                if(RETOUR.Touched()){
+                   while( RETOUR.Touched());
+                   etat=DEMO;
+                }
             }
-            Ack_strat = 0;
             break;
-            
+                 
+           
         case SELECT_SIDE :
             lcd.Clear(LCD_COLOR_WHITE);
             lcd.SetBackColor(LCD_COLOR_WHITE);
@@ -493,6 +752,7 @@
                 canProcessRx();
                 if(COTE_BLEU.Touched()) 
                 {
+                    liaison_Tx.envoyer(0x30,3,"123");
                     Cote = 0x0;
                     InversStrat = Cote;
                     etat = TACTIQUE;
@@ -758,7 +1018,7 @@
             led3=1;
             
             gameEtat = ETAT_GAME_WAIT_FOR_JACK;
-            if (etat == DEMO_EN_COURS) 
+            if (etat == TEST_TELEMETRE|| etat ==TEST_CAPTEURS || etat == TEST_SERVO || etat ==TEST_TIR || etat == DEMO_IMMEUBLE) 
             {
                 SendRawId(DEBUG_FAKE_JAKE);
             }
@@ -1105,7 +1365,7 @@
                             
                             if (modeTelemetre == 0){
                                 if (telemetreDistance == 0){
-                                    waitingAckID = ACK_ACTION;// ack de type action  
+                                    waitingAckID = ACK_FIN_ACTION;// ack de type action  
                                     waitingAckFrom = 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
@@ -1135,8 +1395,8 @@
                     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;
@@ -1318,7 +1578,10 @@
 {
     static signed char FIFO_occupation=0,FIFO_max_occupation=0;
     char useless1 = 0;
-
+    char message[10]="toto";
+    char message1[10]="toto";
+    char message2[10]="toto";
+    char message3[10]="toto";
     FIFO_occupation=FIFO_ecriture-FIFO_lecture;
     if(FIFO_occupation<0)
         FIFO_occupation=FIFO_occupation+SIZE_FIFO;
@@ -1382,7 +1645,7 @@
             /////////////////////////////////////////////Acknowledges de la fin d'action/////////////////////////////////////////////////  
             case INSTRUCTION_END_BALISE:
             case INSTRUCTION_END_MOTEUR:
-            case INSTRUCTION_END_HERKULEX:
+            case ACK_FIN_ACTION:
                 if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id && ((unsigned short)msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8) == waitingAckID)) {
                     //SendRawId(waitingAckID);
                     waitingAckFrom = 0;
@@ -1484,28 +1747,35 @@
                 waitingAckID = 0;
                 break;
                 
-            case RECEPTION_RECALAGE: // BEURK 
+            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]);
-                /*telemetreDistance= (float)telemetreDistance*3.3*1159.6-687.5+98;
-                telemetreDistance1= (float)telemetreDistance1*3.3*1159.6-687.5+98;
-                telemetreDistance2= (float)telemetreDistance2*3.3*1159.6-687.5+98;
-                telemetreDistance3= (float)telemetreDistance3*3.3*1159.6-687.5+98;
-                telemetreDistance-=170;
-                telemetreDistance1-=170;
-                telemetreDistance2-=170;
-                telemetreDistance3-=170;*/
-                
-                /*telemetreDistance= (float)telemetreDistance*100*35.5+50;
-                telemetreDistance1= (float)telemetreDistance1*100*35.5+50;
-                telemetreDistance2= (float)telemetreDistance2*100*35.5+50;
-                telemetreDistance3= (float)telemetreDistance3*100*35.5+50;*/
-                
-                
                 waitingAckFrom = 0;
                 waitingAckID = 0;
+                if(ModeDemo==1){
+                    sprintf(message,"%04d mm",telemetreDistance);
+                    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);
+                    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);
+                    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);
+                    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);
+                    
+                }
                 break;
             
             case RECEPTION_COULEUR: