Revision 10:1964bb91b925, committed 2020-03-03

Comitter:

gabrieltetar

Date:

Tue Mar 03 16:41:33 2020 +0000

Parent:

9:5c8be2a80d35

Child:

11:848ea39838bf

Commit message:

restructred demo menu - features added

Changed in this revision

--- a/Debug/debug.cpp	Mon Feb 10 08:07:17 2020 +0000
+++ b/Debug/debug.cpp	Tue Mar 03 16:41:33 2020 +0000
@@ -1,6 +1,13 @@
 #include "global.h"
 
-
+//debug audio
+void Debug_Audio(unsigned char repertoire, unsigned char track){
+    unsigned char data[3];
+    data[0]=0x01;
+    data[1]=repertoire;
+    data[2]=track;
+    SendMsgCan(SOMO, data,3);
+}
 //azezaea
 
 void debug_Instruction(struct S_Instruction instruction)

--- a/Debug/debug.h	Mon Feb 10 08:07:17 2020 +0000
+++ b/Debug/debug.h	Tue Mar 03 16:41:33 2020 +0000
@@ -2,7 +2,7 @@
 #define CRAC_DEBUG
 //debug
 #include "global.h"
-
+void Debug_Audio(unsigned char repertoire, unsigned char track);
 void debug_Instruction(struct S_Instruction instruction);
 
 char* InstructionTypeToString(enum E_InstructionType type);

--- a/Evitement/Evitement.cpp	Mon Feb 10 08:07:17 2020 +0000
+++ b/Evitement/Evitement.cpp	Tue Mar 03 16:41:33 2020 +0000
@@ -38,7 +38,7 @@
     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;
@@ -74,6 +74,7 @@
 /* DESCRIPTION  : FIFO -> BALISE_END_DANGER                                             */
 /****************************************************************************************/
 unsigned short balise_end_danger(S_Instruction* instruction,S_Dodge_queue* dodgeq, E_stratGameEtat* gameEtat, 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;

--- a/Globals/ident_crac.lib	Mon Feb 10 08:07:17 2020 +0000
+++ b/Globals/ident_crac.lib	Tue Mar 03 16:41:33 2020 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/teams/CRAC-Team/code/ident_crac/#3d8d4041ce79
+https://os.mbed.com/teams/CRAC-Team/code/ident_crac/#b3e08bf1288d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Robots/Strategie_big.cpp	Tue Mar 03 16:41:33 2020 +0000
@@ -0,0 +1,305 @@
+#include "global.h"
+#ifdef ROBOT_BIG
+
+unsigned short x;
+unsigned short y;
+unsigned char isStopEnable = 1;//Permet de savoir si il faut autoriser le stop via les balises
+
+/****************************************************************************************/
+/* FUNCTION NAME: doFunnyAction                                                         */
+/* DESCRIPTION  : Permet de faire la funny action en fin de partie                      */
+/****************************************************************************************/
+void doFunnyAction(void)
+{
+    //envoie de la funny action
+    // 0x007, 01, 01
+    CANMessage msgTx=CANMessage();
+    msgTx.id=GLOBAL_FUNNY_ACTION;
+    msgTx.format=CANStandard;
+    msgTx.type=CANData;
+    msgTx.len=2;
+    msgTx.data[0]=0x01;
+    msgTx.data[1]=0x01;
+    can1.write(msgTx);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: doAction                                                              */
+/* DESCRIPTION  : Effectuer une action specifique                                       */
+/****************************************************************************************/
+unsigned char doAction(unsigned char id, unsigned short var1, short var2)
+{
+    CANMessage msgTx=CANMessage();
+    msgTx.format=CANStandard;
+    msgTx.type=CANData;
+    //affichage_debug(id);
+
+    switch(id) {
+        /////////////////////////////////////////////////////////100 à 108 : ACTIONS HERKULEX/////////////////////////////////////////////
+        case 118:
+            SendRawId(VENTOUSE_AV_CENTRE_BALANCE);
+            break;
+
+        case 201:
+            unsigned char var_tempo;
+            var_tempo = (unsigned char)var1;//0auto 1forceon 2 forceoff
+            SendMsgCan(ASCENSEUR, &var_tempo,1);
+            waitingAckFrom = 0;
+            waitingAckID =0;
+            break;
+
+        case 202:
+            msgTx.id=VIDER_CONVOYEUR;
+            msgTx.len=1;
+            msgTx.data[0]=(unsigned char)var1; //
+            can2.write(msgTx);
+            break;
+
+        case 203:
+            x = var1;
+            if(InversStrat == 1) {
+                y = 3000 - var2;
+            } else {
+                y = var2;
+            }
+            Send2Short(GOLDENIUM_AVANT, x, y);
+            break;
+
+        case 204:
+            unsigned char arg_tempo;
+            if(InversStrat == 1) {
+                switch(var1) {
+                    case AV_DROIT:
+                        arg_tempo = AV_GAUCHE;
+                        break;
+                    case AV_GAUCHE:
+                        arg_tempo = AV_DROIT;
+                        break;
+                    default :
+                        arg_tempo =(unsigned char)var1;
+                        break;
+                }
+
+            } else arg_tempo =(unsigned char)var1;
+            SendMsgCan(HACHEUR_RELEASE_ATOM, &arg_tempo,1);
+            waitingAckFrom = 0;
+            waitingAckID =0;
+            break;
+
+        case 205:
+            SendRawId(PRESENTOIR_AVANT);
+            break;
+            
+        case 206:
+            SendMsgCan(RATEAU, (unsigned char*)&var1,1);
+            break;
+
+
+        case 150:
+            SCORE_GR+=var1;
+            SCORE_GLOBAL=SCORE_GR+SCORE_PR;
+            //liaison_Tx.envoyer_short(0x30,SCORE_GLOBAL);
+            waitingAckFrom = 0;
+            waitingAckID = 0;
+            break;
+
+
+        case 11://0 Désactiver le stop,1 Activer le stop saut de strat,2 Activer le stop avec evitement
+            isStopEnable =(unsigned char) var1;
+            // SendMsgCan(0x5BC, &isStopEnable,1);
+            waitingAckFrom = 0;
+            waitingAckID =0;
+            break;
+
+        case 20://Désactiver l'asservissement
+            setAsservissementEtat(0);
+            break;
+
+        case 21://Activer l'asservissement
+            setAsservissementEtat(1);
+            break;
+
+        case 22://Changer la vitesse du robot
+            SendSpeed(var1);
+            waitingAckFrom = 0;
+            waitingAckID = 0;
+            wait(0.2);
+            break;
+
+        case 23:
+            SendAccel((unsigned short)var1,(unsigned short)var2);//,(unsigned short)arg2, (unsigned short)arg2);
+            wait_us(200);
+            waitingAckFrom = 0;
+            waitingAckID = 0;
+            break;
+
+        case 30://Action tempo
+            wait_ms(var1);
+            waitingAckFrom = 0;
+            waitingAckID = 0;
+            break;
+
+
+
+        default:
+            return 0;//L'action n'existe pas, il faut utiliser le CAN
+
+    }
+    return 1;//L'action est spécifique.
+
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: initRobot                                                             */
+/* DESCRIPTION  : initialiser le robot                                                  */
+/****************************************************************************************/
+void initRobot(void)
+{
+    //Enregistrement de tous les AX12 présent sur la carte
+    /*AX12_register(5,  AX12_SERIAL2);
+    AX12_register(18, AX12_SERIAL2);
+    AX12_register(13, AX12_SERIAL2);
+    AX12_register(1,  AX12_SERIAL1);
+    AX12_register(11,  AX12_SERIAL1);
+    AX12_register(8,  AX12_SERIAL1);
+    AX12_register(7,  AX12_SERIAL2);*/
+
+    //AX12_setGoal(AX12_ID_FUNNY_ACTION, AX12_ANGLE_FUNNY_ACTION_CLOSE,AX12_SPEED_FUNNY_ACTION);
+    //AX12_processChange();
+    //runRobotTest();
+
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: initRobotActionneur                                                   */
+/* DESCRIPTION  : Initialiser la position des actionneurs du robot                      */
+/****************************************************************************************/
+void initRobotActionneur(void)
+{
+    /*doAction(100,1,0);
+    doAction(100,2,0);
+    doAction(110,0,0);
+    doAction(120,0,0);
+    doAction(131,0,0);*/
+
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: runTest                                                               */
+/* DESCRIPTION  : tester l'ensemble des actionneurs du robot                            */
+/****************************************************************************************/
+void runRobotTest(void)
+{
+    /*
+    int waitTime = 500;
+
+    //Test des AX12 dans l'ordre
+    doAction(111,0,0);//Fermeture pince arrière haute
+    wait_ms(waitTime);
+    doAction(110,0,0);//Ouverture pince arrière haute
+    wait_ms(waitTime);
+    doAction(113,0,0);//Fermeture pince arrière basse
+    wait_ms(waitTime);
+    doAction(112,0,0);//Ouverture pince arrière basse
+    wait_ms(waitTime);
+    doAction(115,0,0);//Fermeture porte arrière
+    wait_ms(waitTime);
+    doAction(114,0,0);//Ouverture porte arrière
+    wait_ms(waitTime);
+    doAction(101,0,0);//Fermer les portes avant
+    wait_ms(waitTime);
+    doAction(100,0,0);//Ouvrir les portes avant
+    wait_ms(waitTime);
+    doAction(103,0,0);//Descendre le peigne
+    wait_ms(waitTime);
+    doAction(102,0,0);//Remonter le peigne*/
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: SelectStrategy                                                        */
+/* DESCRIPTION  : Charger le fichier de stratégie correspondante à un id                */
+/* RETURN       : 0=> Erreur, 1=> OK si le fichier existe                               */
+/****************************************************************************************/
+/*int SelectStrategy(unsigned char id)
+{
+    switch(id)
+    {
+        // strat de match
+        case 1:
+            strcpy(cheminFileStart,"/local/strat1.txt");
+            return FileExists(cheminFileStart);
+        case 2:
+            strcpy(cheminFileStart,"/local/strat2.txt");
+            return FileExists(cheminFileStart);
+        case 3:
+            strcpy(cheminFileStart,"/local/strat3.txt");
+            return FileExists(cheminFileStart);
+        case 4:
+            strcpy(cheminFileStart,"/local/strat4.txt");
+            return FileExists(cheminFileStart);
+        case 5:
+            strcpy(cheminFileStart,"/local/strat5.txt");
+            return FileExists(cheminFileStart);
+        case 6:
+            strcpy(cheminFileStart,"/local/strat6.txt");
+            return FileExists(cheminFileStart);
+        case 7:
+            strcpy(cheminFileStart,"/local/strat7.txt");
+            return FileExists(cheminFileStart);
+        case 8:
+            strcpy(cheminFileStart,"/local/strat8.txt");
+            return FileExists(cheminFileStart);
+        case 9:
+            strcpy(cheminFileStart,"/local/strat9.txt");
+            return FileExists(cheminFileStart);
+        case 10:
+            strcpy(cheminFileStart,"/local/strat10.txt");
+            return FileExists(cheminFileStart);
+
+        // strat de demo
+        case 0x10:
+            strcpy(cheminFileStart,"/local/moteur.txt");
+            return FileExists(cheminFileStart);
+        case 0x11:
+#ifdef ROBOT_BIG
+            strcpy(cheminFileStart,"/local/bras.txt");
+#else
+            strcpy(cheminFileStart,"/local/porteAvant.txt");
+#endif
+            return FileExists(cheminFileStart);
+        case 0x12:
+#ifdef ROBOT_BIG
+            strcpy(cheminFileStart,"/local/balancier.txt");
+#else
+            strcpy(cheminFileStart,"/local/mainTourneuse.txt");
+#endif
+            return FileExists(cheminFileStart);
+        default:
+            strcpy(cheminFileStart,"/local/strat1.txt");
+            return 0;
+    }
+}*/
+
+/****************************************************************************************/
+/* FUNCTION NAME: needToStop                                                            */
+/* DESCRIPTION  : Savoir si il faut autoriser le stop du robot via balise               */
+/****************************************************************************************/
+unsigned char needToStop(void)
+{
+    return isStopEnable;
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: doBeforeEndAction                                                     */
+/* DESCRIPTION  : Terminer les actions du robot 1s avant la fin du match                */
+/****************************************************************************************/
+void doBeforeEndAction(void)
+{
+    doAction(110,0,0);//Ouverture pince arrière haute
+    doAction(112,0,0);//Ouverture pince arrière basse
+    doAction(114,0,0);//Ouverture porte arrière
+    doAction(100,0,0);//Ouvrir les portes avant
+    doAction(102,0,0);//Remonter le peigne
+}
+
+#endif

--- a/Robots/Strategie_small.cpp	Mon Feb 10 08:07:17 2020 +0000
+++ b/Robots/Strategie_small.cpp	Tue Mar 03 16:41:33 2020 +0000
@@ -1,5 +1,7 @@
 #include "global.h"
 
+#ifdef ROBOT_SMALL
+
 unsigned short distance_recalage;
 unsigned short distance_revenir;
 
@@ -229,3 +231,5 @@
 {
 
 }
+
+#endif

--- a/Strategie/Strategie.cpp	Mon Feb 10 08:07:17 2020 +0000
+++ b/Strategie/Strategie.cpp	Tue Mar 03 16:41:33 2020 +0000
@@ -118,7 +118,7 @@
 float cptf;
 int cpt,cpt1;
 
-typedef enum {INIT, ATT, CHOIX, DEMO, TEST_TELEMETRE, TEST_COULEUR, TEST_SERVO_BRAS, TEST_VENTOUSE, SELECT_SIDE, TACTIQUE, DETAILS,LECTURE, LAUNCH, AFF_WAIT_JACK, WAIT_JACK, COMPTEUR, FIN} T_etat;
+typedef enum {INIT, ATT, CHOIX, DEMO, TEST_TELEMETRE, TEST_COULEUR, TEST_SERVO_BRAS, TEST_VENTOUSE, TEST_ELECTROV, 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     memGameEtat= gameEtat;
@@ -133,15 +133,21 @@
 Button CHECK (0, 420, 400, 200, "Valider");
 Button MATCH (0, 50, 400, 320, "Match");
 Button DEMONSTRATION (0, 400, 400, 320, "Demo");
-
+//menu demo
 Button TEST_VENT(0, 25, 400, 100, "Test ventouses");
 Button TEST_TELE(0, 135, 400, 100, "Test telemetre");
 Button TEST_COUL(0,245,400,100,"Test capteurs couleur");
 Button TEST_BRAS(0,355,400,100,"Test Bras");
-
-Button TEST_BRAS_GABARIT(0,25,400,100,"Test Gabarit");
-Button TEST_BRAS_GABARITD(0,135,400,100,"Test Gabarit deploye");
-Button TEST_BRAS_PRESENTOIR(0,245,400,100,"Test Presentoir");
+Button TEST_AUDIO(0,465,400,100,"Test Audio");
+Button TEST_ELECTRV(0,465,400,100,"Test Electro Vannes");
+//menu demo/demo/
+Button TEST_A(0,25,195,100,"A");
+Button TEST_B(205,25,195,100,"B");
+Button TEST_C(0,135,195,100,"C");
+Button TEST_D(205,135,195,100,"D");
+Button TEST_NUM(0,245,400,100,"Bras num");
+Button TEST_PLUS(205,355,195,100,"+");
+Button TEST_MOINS(0,355,195,100,"-");
 
 
 Button FORCE_LAUNCH(0, 50, 400, 320, "Force Launch");
@@ -275,7 +281,7 @@
 
 void Setflag(void)
 {
-    flagSendCan = 1;
+    flagSendCan = 1;//gab le zgeg
 }
 
 
@@ -325,7 +331,9 @@
 /****************************************************************************************/
 void automate_etat_ihm(void)
 {
+    char toto[2]; 
     int j;
+    unsigned char maximilien=1;
     if (j==0) {
         ts.Init(lcd.GetXSize(), lcd.GetYSize());
         j++;
@@ -390,6 +398,8 @@
             break;
 
         case DEMO :
+            lcd.SetBackColor(LCD_COLOR_WHITE);
+            lcd.SetTextColor(LCD_COLOR_BLACK);
             lcd.Clear (LCD_COLOR_WHITE);
             lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"DEMO", LEFT_MODE);
             RETOUR.Draw(0xFFFF0000, 0);
@@ -397,6 +407,8 @@
             TEST_TELE.Draw(VERT, 0);
             TEST_COUL.Draw(VERT, 0);
             TEST_BRAS.Draw(VERT, 0);
+            TEST_AUDIO.Draw(VERT, 0);
+            TEST_ELECTRV.Draw(VERT, 0);
             while (etat == DEMO) {
                 canProcessRx();
                 if(TEST_VENT.Touched()) {
@@ -405,12 +417,19 @@
                 } else if(TEST_TELE.Touched()) {
                     while(TEST_TELE.Touched());
                     etat = TEST_TELEMETRE;
+                } else if(TEST_ELECTRV.Touched()) {
+                    while(TEST_ELECTRV.Touched());
+                    etat = TEST_ELECTROV;
                 } else if (TEST_COUL.Touched()) {
                     while(TEST_COUL.Touched());
                     etat =TEST_COULEUR ;
                 } else if (TEST_BRAS.Touched()) {
                     while(TEST_BRAS.Touched());
                     etat =TEST_SERVO_BRAS ;
+                } else if(TEST_AUDIO.Touched()) {
+                    while(TEST_AUDIO.Touched());
+                    TEST_AUDIO.Draw(VERT, 0);
+                    Debug_Audio(3,2);
                 } else if(RETOUR.Touched()) {
                     while(RETOUR.Touched());
                     etat = CHOIX;
@@ -419,38 +438,210 @@
             break;
 
 
-        case TEST_VENTOUSE:      
+        case TEST_VENTOUSE:   
+            lcd.SetBackColor(LCD_COLOR_WHITE);
+            lcd.SetTextColor(LCD_COLOR_BLACK);
+            lcd.Clear (LCD_COLOR_WHITE);   
             lcd.Clear (LCD_COLOR_WHITE);
             lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"Test Ventouse", LEFT_MODE);
+            sprintf(toto,"%hd",maximilien);
+            lcd.DisplayStringAt(50, LINE(20), (unsigned char *)toto, LEFT_MODE);
             RETOUR.Draw(0xFFFF0000,0);
-            while(etat==TEST_VENTOUSE) {
+            TEST_A.Draw(BLEU, BLANC);
+            TEST_NUM.Draw(BLEU, BLANC);
+            TEST_PLUS.Draw(BLEU, BLANC);
+            TEST_MOINS.Draw(BLEU, BLANC);
+            while(etat==TEST_SERVO_BRAS) {
                 if(RETOUR.Touched()) {
                     while (RETOUR.Touched());
                     etat=DEMO;
-                } 
+                } else if(TEST_A.Touched()) {
+                    while (TEST_A.Touched());
+                    SendRawId(TEST_VENT_A);
+                    TEST_A.Draw(BLEU, BLANC);
+                }  else if(TEST_NUM.Touched()) {
+                    while (TEST_NUM.Touched());
+                    TEST_NUM.Draw(BLEU, BLANC);
+                    switch(maximilien){
+                    case 1:
+                    SendRawId(TEST_VENT_1);
+                    break;
+                    case 2:
+                    SendRawId(TEST_VENT_2);
+                    break;
+                    case 3:
+                    SendRawId(TEST_VENT_3);
+                    break;
+                    case 4:
+                    SendRawId(TEST_VENT_4);
+                    break;
+                    case 5:
+                    SendRawId(TEST_VENT_5);
+                    break;
+                    case 6:
+                    SendRawId(TEST_VENT_6);
+                    break;
+                    }
+                } else if(TEST_PLUS.Touched()) {
+                    while (TEST_PLUS.Touched());
+                    TEST_PLUS.Draw(BLEU, BLANC);
+                    if(maximilien!=6)maximilien++;
+                    sprintf(toto,"%hd",maximilien);
+                    lcd.SetBackColor(LCD_COLOR_WHITE);
+                    lcd.SetTextColor(LCD_COLOR_BLACK);
+                    lcd.DisplayStringAt(50, LINE(20), (unsigned char *)toto, LEFT_MODE);
+                } else if(TEST_MOINS.Touched()) {
+                    while (TEST_MOINS.Touched());
+                    TEST_MOINS.Draw(BLEU, BLANC);
+                    if(maximilien!=1)maximilien--;
+                    sprintf(toto,"%hd",maximilien);
+                    lcd.SetBackColor(LCD_COLOR_WHITE);
+                    lcd.SetTextColor(LCD_COLOR_BLACK);
+                    lcd.DisplayStringAt(50, LINE(20), (unsigned char *)toto, LEFT_MODE);
+                }
             }
             break;
-            
-        case TEST_SERVO_BRAS:      
+        
+        case TEST_ELECTROV:
+            lcd.SetBackColor(LCD_COLOR_WHITE);
+            lcd.SetTextColor(LCD_COLOR_BLACK);
+            lcd.Clear (LCD_COLOR_WHITE);   
             lcd.Clear (LCD_COLOR_WHITE);
-            lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"Test Bras", LEFT_MODE);
+            lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"Test Electro Vannes", LEFT_MODE);
+            sprintf(toto,"%hd",maximilien);
+            lcd.DisplayStringAt(50, LINE(20), (unsigned char *)toto, LEFT_MODE);
             RETOUR.Draw(0xFFFF0000,0);
-            TEST_BRAS_PRESENTOIR.Draw(BLEU, BLANC);
-            TEST_BRAS_GABARIT.Draw(BLEU, BLANC);
-            TEST_BRAS_GABARITD.Draw(BLEU, BLANC);
+            TEST_A.Draw(BLEU, BLANC);
+            TEST_NUM.Draw(BLEU, BLANC);
+            TEST_PLUS.Draw(BLEU, BLANC);
+            TEST_MOINS.Draw(BLEU, BLANC);
             while(etat==TEST_SERVO_BRAS) {
                 if(RETOUR.Touched()) {
                     while (RETOUR.Touched());
                     etat=DEMO;
-                } else if(TEST_BRAS_PRESENTOIR.Touched()) {
-                    while (TEST_BRAS_PRESENTOIR.Touched());
-                    SendRawId(PRESENTOIR);
-                } else if(TEST_BRAS_GABARIT.Touched()) {
-                    while (TEST_BRAS_GABARIT.Touched());
-                    SendRawId(GABARIT);
-                } else if(TEST_BRAS_GABARITD.Touched()) {
-                    while (TEST_BRAS_GABARITD.Touched());
-                    SendRawId(GABARIT_D);
+                } else if(TEST_A.Touched()) {
+                    while (TEST_A.Touched());
+                    SendRawId(TEST_ELECTROV_A);
+                    TEST_A.Draw(BLEU, BLANC);
+                }  else if(TEST_NUM.Touched()) {
+                    while (TEST_NUM.Touched());
+                    TEST_NUM.Draw(BLEU, BLANC);
+                    switch(maximilien){
+                    case 1:
+                    SendRawId(TEST_ELECTROV_1);
+                    break;
+                    case 2:
+                    SendRawId(TEST_ELECTROV_2);
+                    break;
+                    case 3:
+                    SendRawId(TEST_ELECTROV_3);
+                    break;
+                    case 4:
+                    SendRawId(TEST_ELECTROV_4);
+                    break;
+                    case 5:
+                    SendRawId(TEST_ELECTROV_5);
+                    break;
+                    case 6:
+                    SendRawId(TEST_ELECTROV_6);
+                    break;
+                    }
+                } else if(TEST_PLUS.Touched()) {
+                    while (TEST_PLUS.Touched());
+                    TEST_PLUS.Draw(BLEU, BLANC);
+                    if(maximilien!=6)maximilien++;
+                    sprintf(toto,"%hd",maximilien);
+                    lcd.SetBackColor(LCD_COLOR_WHITE);
+                    lcd.SetTextColor(LCD_COLOR_BLACK);
+                    lcd.DisplayStringAt(50, LINE(20), (unsigned char *)toto, LEFT_MODE);
+                } else if(TEST_MOINS.Touched()) {
+                    while (TEST_MOINS.Touched());
+                    TEST_MOINS.Draw(BLEU, BLANC);
+                    if(maximilien!=1)maximilien--;
+                    sprintf(toto,"%hd",maximilien);
+                    lcd.SetBackColor(LCD_COLOR_WHITE);
+                    lcd.SetTextColor(LCD_COLOR_BLACK);
+                    lcd.DisplayStringAt(50, LINE(20), (unsigned char *)toto, LEFT_MODE);
+                }
+            }
+            break;
+        
+        case TEST_SERVO_BRAS:  
+              
+            lcd.SetBackColor(LCD_COLOR_WHITE);
+            lcd.SetTextColor(LCD_COLOR_BLACK);
+            lcd.Clear (LCD_COLOR_WHITE);   
+            lcd.Clear (LCD_COLOR_WHITE);
+            lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"Test Bras", LEFT_MODE);
+            sprintf(toto,"%hd",maximilien);
+            lcd.DisplayStringAt(50, LINE(20), (unsigned char *)toto, 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);
+            TEST_NUM.Draw(BLEU, BLANC);
+            TEST_PLUS.Draw(BLEU, BLANC);
+            TEST_MOINS.Draw(BLEU, BLANC);
+            while(etat==TEST_SERVO_BRAS) {
+                if(RETOUR.Touched()) {
+                    while (RETOUR.Touched());
+                    etat=DEMO;
+                } else if(TEST_A.Touched()) {
+                    while (TEST_A.Touched());
+                    SendRawId(TEST_BRAS_A);
+                    TEST_A.Draw(BLEU, BLANC);
+                } else if(TEST_B.Touched()) {
+                    while (TEST_B.Touched());
+                    SendRawId(TEST_BRAS_B);
+                    TEST_B.Draw(BLEU, BLANC);
+                } else if(TEST_C.Touched()) {
+                    while (TEST_C.Touched());
+                    SendRawId(TEST_BRAS_C);
+                    TEST_C.Draw(BLEU, BLANC);
+                } else if(TEST_D.Touched()) {
+                    while (TEST_D.Touched());
+                    SendRawId(TEST_BRAS_D);
+                    TEST_D.Draw(BLEU, BLANC);
+                }  else if(TEST_NUM.Touched()) {
+                    while (TEST_NUM.Touched());
+                    TEST_NUM.Draw(BLEU, BLANC);
+                    switch(maximilien){
+                    case 1:
+                    SendRawId(TEST_BRAS_1);
+                    break;
+                    case 2:
+                    SendRawId(TEST_BRAS_2);
+                    break;
+                    case 3:
+                    SendRawId(TEST_BRAS_3);
+                    break;
+                    case 4:
+                    SendRawId(TEST_BRAS_4);
+                    break;
+                    case 5:
+                    SendRawId(TEST_BRAS_5);
+                    break;
+                    case 6:
+                    SendRawId(TEST_BRAS_6);
+                    break;
+                    }
+                } else if(TEST_PLUS.Touched()) {
+                    while (TEST_PLUS.Touched());
+                    TEST_PLUS.Draw(BLEU, BLANC);
+                    if(maximilien!=6)maximilien++;
+                    sprintf(toto,"%hd",maximilien);
+                    lcd.SetBackColor(LCD_COLOR_WHITE);
+                    lcd.SetTextColor(LCD_COLOR_BLACK);
+                    lcd.DisplayStringAt(50, LINE(20), (unsigned char *)toto, LEFT_MODE);
+                } else if(TEST_MOINS.Touched()) {
+                    while (TEST_MOINS.Touched());
+                    TEST_MOINS.Draw(BLEU, BLANC);
+                    if(maximilien!=1)maximilien--;
+                    sprintf(toto,"%hd",maximilien);
+                    lcd.SetBackColor(LCD_COLOR_WHITE);
+                    lcd.SetTextColor(LCD_COLOR_BLACK);
+                    lcd.DisplayStringAt(50, LINE(20), (unsigned char *)toto, LEFT_MODE);
                 }
             }
             break;
@@ -772,6 +963,7 @@
             SendRawId(GLOBAL_START);
 
             gameEtat = ETAT_GAME_WAIT_FOR_JACK;
+            Debug_Audio(3,7);
             if (etat == TEST_TELEMETRE|| etat ==TEST_VENTOUSE || etat == TEST_COULEUR || etat ==TEST_SERVO_BRAS) {
                 SendRawId(DEBUG_FAKE_JAKE);
             } else {
@@ -905,7 +1097,7 @@
 
             break;
         case ETAT_GAME_START:
-
+            Debug_Audio(3,3);
             gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
 
             if (ModeDemo == 0) {
@@ -1016,6 +1208,7 @@
                     break;
                 }
                 case MV_COURBURE://C'est un rayon de courbure
+                    Debug_Audio(3,6);
                     float alpha=0, theta=0;
                     unsigned short alph=0;
                     actionPrecedente = MV_COURBURE;
@@ -1169,6 +1362,7 @@
 
 
                 case MV_LINE://Ligne droite
+                    Debug_Audio(3,8);
                     waitingAckID = ASSERVISSEMENT_RECALAGE;
                     waitingAckFrom = ACKNOWLEDGE_MOTEUR;
                     if(instruction.nextActionType == ENCHAINEMENT) {
@@ -1191,6 +1385,7 @@
 
                     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;
@@ -1217,6 +1412,7 @@
 
                     break;
                 case MV_XYT:
+                    Debug_Audio(3,10);
                     if(instruction.direction == BACKWARD) {
                         localData1 = -1;
                     } else {
@@ -1286,11 +1482,10 @@
                 
                     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
-                    tempo = doAction(instruction.arg1,instruction.arg2,instruction.arg3);
+                    //tempo = doAction(instruction.arg1,instruction.arg2,instruction.arg3);
                     //  unsigned char test=(unsigned char) tempo;
                     // SendMsgCan(0x5BD, &test,1);
                     if(tempo == 1) {
@@ -1486,6 +1681,7 @@
             break;
         
         case ETAT_END:
+            Debug_Audio(3,4);
             if (ModeDemo) {
                 gameEtat = ETAT_CHECK_CARTE_SCREEN;
                 ModeDemo = 1;
@@ -1752,6 +1948,10 @@
                 }
 
                 break;
+                case SOMO:
+                break;
+                default:
+                break;
                 /*
                             case NO_BLOC: //il n'y a pas de bloc, on saute les étapes liées à l'attrape bloc
                                 actual_instruction = instruction.nextLineError;

--- a/main.cpp	Mon Feb 10 08:07:17 2020 +0000
+++ b/main.cpp	Tue Mar 03 16:41:33 2020 +0000
@@ -69,7 +69,7 @@
     lcd.DisplayStringAt(0, 0,(uint8_t *)"Initialisation", LEFT_MODE);
     led1 = 1;
     bluetooth_init();
-    //lecture_fichier(); //bloquant si pas de carte SD
+    lecture_fichier(); //bloquant si pas de carte SD
     led1 = 0;
     wait_ms(2000);//Attente pour que toutes les cartes se lancent et surtout le CANBlue
     while(true) {