Revision 12:14729d584500, committed 2016-05-09

Comitter:

antbig

Date:

Mon May 09 09:10:17 2016 +0000

Parent:

11:ed13a480ddca

Child:

13:93edbb03a8c6

Commit message:

1Version utilis? lors du match 5

Changed in this revision

--- a/AX12-V2/AX12-V2.cpp	Mon May 02 19:40:59 2016 +0000
+++ b/AX12-V2/AX12-V2.cpp	Mon May 09 09:10:17 2016 +0000
@@ -105,7 +105,7 @@
 /****************************************************************************************/
 void AX12_notifyCANEnd(unsigned char id) 
 {
-    if(waitingAckFrom == SERVO_AX12_DONE) {
+    if(waitingAckFrom == SERVO_AX12_DONE && waitingAckID == id) {
         waitingAckFrom = 0;
         waitingAckID = 0;
     }
@@ -150,7 +150,7 @@
     int i=0;
     int dataToSendLength = 0;
     char dataToSend[100];
-
+    int sendTwice = 0;
     
     for(i=0;i<lastAX12Use;i++)
     {
@@ -179,11 +179,13 @@
     //printf("need to send %d data\n",dataToSendLength);
     if(dataToSendLength > 0)//Il y a des données à envoyer en local
     {
-        AX12_syncWrite(AX12_Serial1, AX12_REG_GOAL_POSITION, dataToSendLength, dataToSend);
-        //wait_ms(10);
-        AX12_syncWrite(AX12_Serial2, AX12_REG_GOAL_POSITION, dataToSendLength, dataToSend);
+        for(sendTwice=0;sendTwice<2;sendTwice++)
+        {
+            AX12_syncWrite(AX12_Serial1, AX12_REG_GOAL_POSITION, dataToSendLength, dataToSend);
+            //wait_ms(10);
+            AX12_syncWrite(AX12_Serial2, AX12_REG_GOAL_POSITION, dataToSendLength, dataToSend);
+        }
     }
-    
 }
 
 /****************************************************************************************/

--- a/Asservissement/Asservissement.cpp	Mon May 02 19:40:59 2016 +0000
+++ b/Asservissement/Asservissement.cpp	Mon May 09 09:10:17 2016 +0000
@@ -156,3 +156,40 @@
 
     can1.write(msgTx);
 }
+
+/****************************************************************************************/
+/* FUNCTION NAME: setAsservissementEtat                                                 */
+/* DESCRIPTION  : Activer ou désactiver l'asservissement                                */
+/****************************************************************************************/
+void setAsservissementEtat(unsigned char enable)
+{
+    CANMessage msgTx=CANMessage();
+    msgTx.id=ASSERVISSEMENT_ENABLE;  // Tx rotation autour du centre du robot
+    msgTx.len=1;
+    msgTx.format=CANStandard;
+    msgTx.type=CANData;
+    //  Angle signé sur 2 octets
+    msgTx.data[0]=(unsigned char)((enable==0)?0:1);
+
+    can1.write(msgTx);
+}
+
+
+/****************************************************************************************/
+/* FUNCTION NAME: SendSpeed                                                             */
+/* DESCRIPTION  : Envoie un asservissement paramètre retournant à une vitesse           */
+/****************************************************************************************/
+void SendSpeed (unsigned short vitesse, unsigned short acceleration)
+{
+    CANMessage msgTx=CANMessage();
+    msgTx.id=ASSERVISSEMENT_CONFIG;
+    msgTx.format=CANStandard;
+    msgTx.type=CANData;
+    msgTx.len=4;
+    msgTx.data[0]=(unsigned char)(vitesse&0x00FF);
+    msgTx.data[1]=(unsigned char)((vitesse&0xFF00)>>8);
+    msgTx.data[2]=(unsigned char)(acceleration&0x00FF);
+    msgTx.data[3]=(unsigned char)((acceleration&0xFF00)>>8);
+
+    can1.write(msgTx);
+}
\ No newline at end of file

--- a/Asservissement/Asservissement.h	Mon May 02 19:40:59 2016 +0000
+++ b/Asservissement/Asservissement.h	Mon May 09 09:10:17 2016 +0000
@@ -51,4 +51,15 @@
 
 void SetOdometrie (unsigned short canId, unsigned short x,unsigned short y,signed short theta);
 
+/****************************************************************************************/
+/* FUNCTION NAME: setAsservissementEtat                                                 */
+/* DESCRIPTION  : Activer ou désactiver l'asservissement                                */
+/****************************************************************************************/
+void setAsservissementEtat(unsigned char enable);
+
+/****************************************************************************************/
+/* FUNCTION NAME: SendSpeed                                                             */
+/* DESCRIPTION  : Envoie un asservissement paramètre retournant à une vitesse           */
+/****************************************************************************************/
+void SendSpeed (unsigned short vitesse, unsigned short acceleration);
 #endif
\ No newline at end of file

--- a/Globals/constantes.h	Mon May 02 19:40:59 2016 +0000
+++ b/Globals/constantes.h	Mon May 09 09:10:17 2016 +0000
@@ -6,7 +6,7 @@
 
 
 
-#define SIZE_FIFO               10 //Taille du buffer pour le bus CAN
+#define SIZE_FIFO               25 //Taille du buffer pour le bus CAN
 
 #define SIZE                    750 //Taille d'une ligne du fichier
 #define SIZE_BUFFER_FILE        150 //Taille du buffer d'instruction 
@@ -16,21 +16,27 @@
 /****
 ** 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
 
-    #define NOMBRE_CARTES           3 //Le nombre de carte présente sur le gros robot
+    #define NOMBRE_CARTES           5 //Le nombre de carte présente sur le gros robot
     #define POSITION_DEBUT_X 765
     #define POSITION_DEBUT_Y 100
     #define POSITION_DEBUT_T 900
+    
+    #define BALISE_TIMEOUT 6000
+    
 #else
 
     #define NOMBRE_CARTES           3 //Le nombre de carte présente sur le petit robot
     #define POSITION_DEBUT_X 990
     #define POSITION_DEBUT_Y 150
     #define POSITION_DEBUT_T 0
+    
+    #define BALISE_TIMEOUT 2000
+    
 #endif
 
 

--- a/Globals/ident_crac.h	Mon May 02 19:40:59 2016 +0000
+++ b/Globals/ident_crac.h	Mon May 09 09:10:17 2016 +0000
@@ -1,12 +1,20 @@
+#ifndef CRAC_IDENTH
+#define CRAC_IDENTH
+
+
+
 #define GLOBAL_GAME_END 0x004  // Stop fin du match
 #define GLOBAL_START 0x002  // Start
 #define GLOBAL_END_INIT_POSITION 0x005  // Fin positionnement robot avant depart
 #define GLOBAL_FUNNY_ACTION 0x007  // Funny action start  (0: start, 1: stop)
 
-#define BALISE_STOP 0x003  // Trame stop  (angle en °, Type du robot : 1=>gros robot, 2=> petit)
-#define BALISE_DANGER 0xA  // Trame danger  (angle en °, Type du robot : 1=>gros robot, 2=> petit)
+#define BALISE_STOP 0x003  // Trame stop
+
+#define BALISE_DANGER 0xA  // Trame danger
+
 #define BALISE_END_DANGER 0xB  // Trame fin de danger
 
+
 #define ASSERVISSEMENT_STOP 0x001  // Stop moteur
 #define ASSERVISSEMENT_SPEED_DANGER 0x006  // Vitesse de danger
 #define ASSERVISSEMENT_XYT 0x020  // Asservissement (x,y,theta)  (0 : au choix 1 : avant -1 : arrière)
@@ -19,6 +27,7 @@
 #define ODOMETRIE_BIG_VITESSE 0x027  // Odométrie vitesse  (Indication sur l'état actuel)
 #define ODOMETRIE_SMALL_POSITION 0x028  // Odométrie position robot  (Position actuel du robot)
 #define ODOMETRIE_SMALL_VITESSE 0x029  // Odométrie vitesse  (Indication sur l'état actuel)
+#define ACTION_BIG_DEMARRAGE 0x02A  // Action de départ du GR  (Lancement de la trajectoire de départ du GR)
 
 #define ASSERVISSEMENT_INFO_CONSIGNE 0x1F0  // Info Consigne et Commande moteur
 #define ASSERVISSEMENT_CONFIG_KPP_DROITE 0x1F1  // Config coef KPP_Droit
@@ -29,36 +38,55 @@
 #define ASSERVISSEMENT_CONFIG_KPD_GAUCHE 0x1F6  // Config coef KPD_Gauche
 #define ASSERVISSEMENT_ENABLE 0x1F7  // Activation asservissement  (0 : désactivation, 1 : activation)
 
+
 #define RESET_BALISE 0x030  // Reset balise
 #define RESET_MOTEUR 0x031  // Reset moteur
 #define RESET_IHM 0x032  // Reset écran tactile
 #define RESET_ACTIONNEURS 0x033  // Reset actionneurs
 #define RESET_POMPES 0x034  // Reset pompes
+#define RESET_AX12 0x035  // Reset AX12
+
+
+
+
 #define RESET_STRAT 0x3A  // Reset stratégie
 
+
 #define CHECK_BALISE 0x060  // Check balise
 #define CHECK_MOTEUR 0x061  // Check moteur
 #define CHECK_IHM 0x062  // Check écran tactile
 #define CHECK_ACTIONNEURS 0x063  // Check actionneurs
 #define CHECK_POMPES 0x064  // Check pompes
+#define CHECK_AX12 0x065  // Check AX12
+
+
+
+
 
 #define ALIVE_BALISE 0x070  // Alive balise
 #define ALIVE_MOTEUR 0x071  // Alive moteur
 #define ALIVE_IHM 0x072  // Alive écran tactile
 #define ALIVE_ACTIONNEURS 0x073  // Alive actionneurs
 #define ALIVE_POMPES 0x074  // Alive pompes
+#define ALIVE_AX12 0x075  // Alive AX12
+
+
+
+
 
 #define ACKNOWLEDGE_BALISE 0x100  // Acknowledge balise
 #define ACKNOWLEDGE_MOTEUR 0x101  // Acknowledge moteur
 #define ACKNOWLEDGE_IHM 0x102  // Acknowledge ecran tactile
 #define ACKNOWLEDGE_ACTIONNEURS 0x103  // Acknowledge actionneurs
-#define ACKNOWLEDGE_ 0x104  // Acknowledge pompes
+#define ACKNOWLEDGE_POMPES 0x104  // Acknowledge pompes
+#define ACKNOWLEDGE_STRAT 0x10A  // Acknowledge pompes
 
 #define INSTRUCTION_END_BALISE 0x110  // Fin instruction balise  (Indique que l'instruction est terminée)
 #define INSTRUCTION_END_MOTEUR 0x111  // Fin instruction moteur  (Indique que l'instruction est terminée)
 #define INSTRUCTION_END_IHM 0x112  // Fin instruction ecran tactile  (Indique que l'instruction est terminée)
 #define INSTRUCTION_END_ACTIONNEURS 0x113  // Fin instruction actionneurs  (Indique que l'instruction est terminée)
 
+
 #define ECRAN_CHOICE_STRAT 0x601  // Choix d'une stratégie  (n° strat (1-4))
 #define ECRAN_CHOICE_COLOR 0x602  // Couleur  (0->Purple;1->green)
 #define ECRAN_START_MATCH 0x603  // Match  (Indique que l'on souhaite commencer le match)
@@ -72,6 +100,8 @@
 #define ECRAN_PRINTF_3 0x6C2  // Tactile printf  (Afficher les 8 troisième caractères)
 #define ECRAN_PRINTF_4 0x6C3  // Tactile printf  (Afficher les 8 quatrième caractères)
 #define ECRAN_PRINTF_CLEAR 0x6CF  // Tactile printf clear  (Permet d'effacer l'ecran)
+#define ECRAN_CHOICE_START_ACTION 0x604  // Tactile printf clear  (Choisir si il faut lancer le test actionneur)
+#define ECRAN_ACK_CHOICE_START_ACTION 0x605  // Tactile printf clear  (Ack du test actionneur)
 
 #define ERROR_OVERFLOW_BALISE 0x040  // Overflow odométrie
 #define ERROR_OVERFLOW_MOTEUR 0x041  // Overflow asservissement
@@ -93,3 +123,5 @@
 #define SERVO_XL320 0x093  // XL320
 
 #define POMPE_PWM 0x9A  // pwm des pompes  (pwm entre 0 et 100)
+    
+#endif

--- a/Robots/Config_big.h	Mon May 02 19:40:59 2016 +0000
+++ b/Robots/Config_big.h	Mon May 09 09:10:17 2016 +0000
@@ -11,7 +11,38 @@
 #define AX12_ID_PORTE_ARRIERE_DROITE        4
 #define AX12_ID_PORTE_ARRIERE_GAUCHE        5
 #define AX12_ID_FUNNY_ACTION                1
+#define AX12_ID_CHARIOT                     2
+#define AX12_ID_PEIGNE                      3
+#define AX12_ID_PORTE_AVANT_DROITE          11
+#define AX12_ID_PORTE_AVANT_GAUCHE          6
+#define AX12_ID_VENTOUSE                    8
+#define AX12_ID_CONE                        7
+
+#define AX12_ANGLE_FUNNY_ACTION_CLOSE       150
+#define AX12_ANGLE_FUNNY_ACTION_OPEN        95
 
 
 
+#define AX12_ANGLE_PORTE_AVANT_GAUCHE_OUVERTE   18
+#define AX12_ANGLE_PORTE_AVANT_GAUCHE_FERMER    120
+#define AX12_ANGLE_PORTE_AVANT_DROITE_OUVERTE   280
+#define AX12_ANGLE_PORTE_AVANT_DROITE_FERMER    183
+
+#define AX12_ANGLE_PEIGNE_UP 60
+#define AX12_ANGLE_PEIGNE_DOWN 120
+
+#define AX12_ANGLE_VENTOUSE_UP      150
+#define AX12_ANGLE_VENTOUSE_DOWN    130
+
+#define AX12_ANGLE_CONE_INSIDE  45
+#define AX12_ANGLE_CONE_OUTSIDE 155
+
+
+
+#define AX12_SPEED_FUNNY_ACTION             100
+#define AX12_SPEED_VENTOUSE                 100
+#define AX12_SPEED_PEIGNE                   500
+
+#define POMPES_PWM 100
+
 #endif
\ No newline at end of file

--- a/Robots/Config_small.h	Mon May 02 19:40:59 2016 +0000
+++ b/Robots/Config_small.h	Mon May 09 09:10:17 2016 +0000
@@ -39,7 +39,7 @@
 #define AX12_SPEED_PALET          0x200
 
 #define XL320_ID_PINCE_DROITE           5
-#define XL320_ID_PINCE_GAUCHE           6
+#define XL320_ID_PINCE_GAUCHE           4
 
 #define XL320_ANGLE_PINCE_DROITE_FERMER     70
 #define XL320_ANGLE_PINCE_DROITE_SECURISE   300

--- a/Robots/StrategieManager.h	Mon May 02 19:40:59 2016 +0000
+++ b/Robots/StrategieManager.h	Mon May 09 09:10:17 2016 +0000
@@ -22,6 +22,12 @@
 void initRobot(void);
 
 /****************************************************************************************/
+/* FUNCTION NAME: initRobotActionneur                                                   */
+/* DESCRIPTION  : Initialiser la position des actionneurs du robot                      */
+/****************************************************************************************/
+void initRobotActionneur(void);
+
+/****************************************************************************************/
 /* FUNCTION NAME: runTest                                                               */
 /* DESCRIPTION  : tester l'ensemble des actionneurs du robot                            */
 /****************************************************************************************/
@@ -34,4 +40,16 @@
 /****************************************************************************************/
 int SelectStrategy(unsigned char id);
 
+/****************************************************************************************/
+/* FUNCTION NAME: needToStop                                                            */
+/* DESCRIPTION  : Savoir si il faut autoriser le stop du robot via balise               */
+/****************************************************************************************/
+unsigned char needToStop(void);
+
+/****************************************************************************************/
+/* FUNCTION NAME: doBeforeEndAction                                                     */
+/* DESCRIPTION  : Terminer les actions du robot 1s avant la fin du match                */
+/****************************************************************************************/
+void doBeforeEndAction(void);
+
 #endif
\ No newline at end of file

--- a/Robots/Strategie_big.cpp	Mon May 02 19:40:59 2016 +0000
+++ b/Robots/Strategie_big.cpp	Mon May 09 09:10:17 2016 +0000
@@ -2,13 +2,14 @@
 #ifdef ROBOT_BIG
 #include "Config_big.h"
 
+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) {
-    AX12_setGoal(AX12_ID_FUNNY_ACTION, 275);
-    AX12_setGoal(AX12_ID_FUNNY_ACTION, 180);
+    AX12_setGoal(AX12_ID_FUNNY_ACTION, AX12_ANGLE_FUNNY_ACTION_OPEN,AX12_SPEED_FUNNY_ACTION);
     AX12_processChange();
     
 }
@@ -18,8 +19,46 @@
 /* DESCRIPTION  : Effectuer une action specifique                                       */
 /****************************************************************************************/
 unsigned char doAction(unsigned char id, unsigned short speed, short angle) {
+    CANMessage msgTx=CANMessage();
     switch(id) {
         
+        case 100://Ouvrir les portes avant
+            AX12_setGoal(AX12_ID_PORTE_AVANT_GAUCHE, AX12_ANGLE_PORTE_AVANT_GAUCHE_OUVERTE);
+            AX12_setGoal(AX12_ID_PORTE_AVANT_DROITE, AX12_ANGLE_PORTE_AVANT_DROITE_OUVERTE);
+            AX12_processChange();
+        break;
+        case 101://Fermer les portes avant
+            AX12_setGoal(AX12_ID_PORTE_AVANT_GAUCHE, AX12_ANGLE_PORTE_AVANT_GAUCHE_FERMER);
+            AX12_setGoal(AX12_ID_PORTE_AVANT_DROITE, AX12_ANGLE_PORTE_AVANT_DROITE_FERMER);
+            AX12_processChange();
+        break;
+        
+        case 102://Remonter le peigne
+            AX12_setGoal(AX12_ID_PEIGNE, AX12_ANGLE_PEIGNE_UP);
+            AX12_processChange();
+        break;
+        case 103://Descendre le peigne
+            AX12_setGoal(AX12_ID_PEIGNE, AX12_ANGLE_PEIGNE_DOWN);
+            AX12_processChange();
+        break;
+        
+        case 104://Monter le support ventouse haut
+            AX12_setGoal(AX12_ID_VENTOUSE, AX12_ANGLE_VENTOUSE_UP);
+            AX12_processChange();
+        break;
+        case 105://Descendre le support ventouse haut
+            AX12_setGoal(AX12_ID_VENTOUSE, AX12_ANGLE_VENTOUSE_DOWN);
+            AX12_processChange();
+        break;
+        case 106://Remonter le support du cone arriere
+            AX12_setGoal(AX12_ID_CONE, AX12_ANGLE_CONE_INSIDE);
+            AX12_processChange();
+        break;
+        case 107://Descendre le support du cone arriere
+            AX12_setGoal(AX12_ID_CONE, AX12_ANGLE_CONE_OUTSIDE);
+            AX12_processChange();
+        break;
+        
         case 110://Ouvrir la pince arrière haute
             AX12_setGoal(AX12_ID_PINCE_ARRIERE_HAUTE_GAUCHE, 205);
             AX12_setGoal(AX12_ID_PINCE_ARRIERE_HAUTE_DROITE, 95);
@@ -29,17 +68,21 @@
             AX12_setGoal(AX12_ID_PINCE_ARRIERE_HAUTE_GAUCHE, 145);
             AX12_setGoal(AX12_ID_PINCE_ARRIERE_HAUTE_DROITE, 155);
             AX12_processChange();
+            /*waitingAckID = AX12_ID_PINCE_ARRIERE_HAUTE_DROITE;
+            waitingAckFrom = SERVO_AX12_DONE;*/
         break;
         
         case 112://Ouvrir la pince arrière basse
-            AX12_setGoal(AX12_ID_PINCE_ARRIERE_BASSE_GAUCHE, 205);
-            AX12_setGoal(AX12_ID_PINCE_ARRIERE_BASSE_DROITE, 95);
+            AX12_setGoal(AX12_ID_PINCE_ARRIERE_BASSE_GAUCHE, 215);
+            AX12_setGoal(AX12_ID_PINCE_ARRIERE_BASSE_DROITE, 85);
             AX12_processChange();
         break;
         case 113://Fermer la pince arrière basse
             AX12_setGoal(AX12_ID_PINCE_ARRIERE_BASSE_GAUCHE, 145);
             AX12_setGoal(AX12_ID_PINCE_ARRIERE_BASSE_DROITE, 155);
             AX12_processChange();
+           /* waitingAckID = AX12_ID_PINCE_ARRIERE_BASSE_DROITE;
+            waitingAckFrom = SERVO_AX12_DONE;*/
         break;
         
         case 114://Ouvrir les portes arrières
@@ -48,10 +91,79 @@
             AX12_processChange();
         break;
         case 115://Fermer les portes arrière
-            AX12_setGoal(AX12_ID_PORTE_ARRIERE_GAUCHE, 145);
-            AX12_setGoal(AX12_ID_PORTE_ARRIERE_DROITE, 155);
+            //AX12_setGoal(AX12_ID_PORTE_ARRIERE_GAUCHE, 145);
+            //AX12_setGoal(AX12_ID_PORTE_ARRIERE_DROITE, 155);
+            AX12_setGoal(AX12_ID_PORTE_ARRIERE_GAUCHE, 142);
+            AX12_setGoal(AX12_ID_PORTE_ARRIERE_DROITE, 158);
+            AX12_processChange();
+            //waitingAckID = AX12_ID_PORTE_ARRIERE_DROITE;
+            //waitingAckFrom = SERVO_AX12_DONE;
+        break;
+        
+        case 120://Activer les pompes
+            AX12_setGoal(AX12_ID_VENTOUSE, AX12_ANGLE_VENTOUSE_UP,AX12_SPEED_VENTOUSE);
             AX12_processChange();
+        
+            
+            msgTx.id=POMPE_PWM;
+            msgTx.format=CANStandard;
+            msgTx.type=CANData;
+            msgTx.len=6;
+        
+            // x sur 2 octets
+            msgTx.data[0]=(unsigned char)POMPES_PWM;
+            msgTx.data[1]=(unsigned char)POMPES_PWM;
+            msgTx.data[2]=(unsigned char)POMPES_PWM;
+            msgTx.data[3]=(unsigned char)POMPES_PWM;
+            msgTx.data[4]=(unsigned char)POMPES_PWM;
+            msgTx.data[5]=(unsigned char)POMPES_PWM;
+        
+            can1.write(msgTx);
         break;
+        case 121://Désactiver les pompes
+            msgTx.id=POMPE_PWM;
+            msgTx.format=CANStandard;
+            msgTx.type=CANData;
+            msgTx.len=6;
+        
+            // x sur 2 octets
+            msgTx.data[0]=(unsigned char)0;
+            msgTx.data[1]=(unsigned char)0;
+            msgTx.data[2]=(unsigned char)0;
+            msgTx.data[3]=(unsigned char)0;
+            msgTx.data[4]=(unsigned char)0;
+            msgTx.data[5]=(unsigned char)0;
+        
+            can1.write(msgTx);
+        break;
+        
+        case 10://Désactiver le stop
+            isStopEnable = 0;
+        break;
+        case 11://Activer le stop
+            isStopEnable = 1;
+        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(speed,(unsigned short)angle);
+        break;
+        
+        case 30://Action tempo
+            wait_ms(speed);
+        break;
+        
+        case 130://Lancer mouvement de sortie de la zone de départ
+            SendRawId(ACTION_BIG_DEMARRAGE);
+            waitingAckID = ACTION_BIG_DEMARRAGE;
+            waitingAckFrom = INSTRUCTION_END_MOTEUR;
+        break;
+        
         default:
             return 0;//L'action n'existe pas, il faut utiliser le CAN
         
@@ -67,35 +179,61 @@
 void initRobot(void) 
 {
     //Enregistrement de tous les AX12 présent sur la carte
-    AX12_register(4,  AX12_SERIAL1);
-    AX12_register(14, AX12_SERIAL1);
-    AX12_register(15, AX12_SERIAL1);
     AX12_register(5,  AX12_SERIAL2);
     AX12_register(18, AX12_SERIAL2);
     AX12_register(13, AX12_SERIAL2);
-    AX12_register(1,  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(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
+    doAction(106,0,0);//Remonter le support du cone arriere
+}
+
+/****************************************************************************************/
 /* 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(500);
+    wait_ms(waitTime);
     doAction(110,0,0);//Ouverture pince arrière haute
-    wait_ms(500);
+    wait_ms(waitTime);
     doAction(113,0,0);//Fermeture pince arrière basse
-    wait_ms(500);
+    wait_ms(waitTime);
     doAction(112,0,0);//Ouverture pince arrière basse
-    wait_ms(500);
+    wait_ms(waitTime);
     doAction(115,0,0);//Fermeture porte arrière
-    wait_ms(500);
+    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
 }
 
 /****************************************************************************************/
@@ -116,10 +254,38 @@
         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);
         default:
-            strcpy(cheminFileStart,"/local/strat.txt");
+            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 May 02 19:40:59 2016 +0000
+++ b/Robots/Strategie_small.cpp	Mon May 09 09:10:17 2016 +0000
@@ -2,6 +2,8 @@
 #ifdef ROBOT_SMALL
 #include "Config_small.h"
 
+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                      */
@@ -41,6 +43,11 @@
                     AX12_ANGLE_BRAS_BASE_DROIT_OUVERT,
                     AX12_SPEED_BRAS_BASE
                 );
+                AX12_setGoal(
+                    AX12_ID_BRAS_RELACHEUR_DROIT,
+                    AX12_ANGLE_BRAS_RELACHEUR_DROIT_REPLIER,
+                    AX12_SPEED_BRAS_RELACHEUR
+                );
                 AX12_processChange();
                 waitingAckID = AX12_ID_BRAS_BASE_DROIT;
                 waitingAckFrom = SERVO_AX12_DONE;
@@ -50,6 +57,11 @@
                     AX12_ANGLE_BRAS_BASE_GAUCHE_OUVERT,
                     AX12_SPEED_BRAS_BASE
                 );
+                AX12_setGoal(
+                    AX12_ID_BRAS_RELACHEUR_GAUCHE,
+                    AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_REPLIER,
+                    AX12_SPEED_BRAS_RELACHEUR
+                );
                 AX12_processChange();
                 waitingAckID = AX12_ID_BRAS_BASE_GAUCHE;
                 waitingAckFrom = SERVO_AX12_DONE;
@@ -121,27 +133,31 @@
         case 105://Rentrer le bras dans le robot, fermer le séparateur
             if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
                 AX12_setGoal(
+                    AX12_ID_BRAS_RELACHEUR_DROIT,
+                    AX12_ANGLE_BRAS_RELACHEUR_DROIT_REPLIER,
+                    AX12_SPEED_BRAS_RELACHEUR
+                );
+                AX12_processChange();
+                wait_ms(100);
+                AX12_setGoal(
                     AX12_ID_BRAS_BASE_DROIT,
                     AX12_ANGLE_BRAS_BASE_DROIT_REPLIER,
                     AX12_SPEED_BRAS_BASE
                 );
-                AX12_setGoal(
-                    AX12_ID_BRAS_RELACHEUR_DROIT,
-                    AX12_ANGLE_BRAS_RELACHEUR_DROIT_REPLIER,
-                    AX12_SPEED_BRAS_RELACHEUR
-                );
                 AX12_processChange();
                 waitingAckID = AX12_ID_BRAS_BASE_DROIT;
                 waitingAckFrom = SERVO_AX12_DONE;
             } else { 
                 AX12_setGoal(
-                    AX12_ID_BRAS_BASE_GAUCHE,
-                    AX12_ANGLE_BRAS_BASE_GAUCHE_REPLIER,
+                    AX12_ID_BRAS_RELACHEUR_GAUCHE,
+                    AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_REPLIER,
                     AX12_SPEED_BRAS_RELACHEUR
                 );
+                AX12_processChange();
+                wait_ms(100);
                 AX12_setGoal(
-                    AX12_ID_BRAS_RELACHEUR_GAUCHE,
-                    AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_REPLIER,
+                    AX12_ID_BRAS_BASE_GAUCHE,
+                    AX12_ANGLE_BRAS_BASE_GAUCHE_REPLIER,
                     AX12_SPEED_BRAS_RELACHEUR
                 );
                 AX12_processChange();
@@ -216,9 +232,9 @@
         
         case 200://ouvir la pince gauche
             if(InversStrat == 1) {
-                XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_FERMER);
+                XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_OUVERTE);
             } else {
-                XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_FERMER);
+                XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_OUVERTE);
             }
         break;
         case 205://Sécuriser le palet gauche
@@ -257,6 +273,18 @@
                 XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_FERMER);
             }
         break;
+        case 10://Désactiver le stop
+            isStopEnable = 0;
+        break;
+        case 11://Activer le stop
+            isStopEnable = 1;
+        break;
+        case 20://Désactiver l'asservissement
+            setAsservissementEtat(0);
+        break;
+        case 21://Activer l'asservissement
+            setAsservissementEtat(1);
+        break;
         
         default:
             return 0;//L'action n'existe pas, il faut utiliser le CAN
@@ -280,6 +308,9 @@
     AX12_register(4,AX12_SERIAL2);
     AX12_register(16,AX12_SERIAL2);
     AX12_register(17,AX12_SERIAL2,0x0FF);
+    
+    //runRobotTest();
+    
     /*
     AX12_setGoal(AX12_ID_BRAS_BASE_INV,278,0x0FF);
     AX12_setGoal(AX12_ID_BRAS_RELACHEUR_INV,160);//fermer le bras
@@ -289,12 +320,143 @@
 }
 
 /****************************************************************************************/
+/* FUNCTION NAME: initRobotActionneur                                                   */
+/* DESCRIPTION  : Initialiser la position des actionneurs du robot                      */
+/****************************************************************************************/
+void initRobotActionneur(void)
+{
+    XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_FERMER);
+    XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_FERMER);
+    
+    AX12_setGoal(
+        AX12_ID_BRAS_RELACHEUR_DROIT,
+        AX12_ANGLE_BRAS_RELACHEUR_DROIT_REPLIER,
+        AX12_SPEED_BRAS_RELACHEUR
+    );
+    AX12_setGoal(
+        AX12_ID_BRAS_RELACHEUR_GAUCHE,
+        AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_REPLIER,
+        AX12_SPEED_BRAS_RELACHEUR
+    );
+    AX12_setGoal(
+        AX12_ID_BRAS_BASE_DROIT,
+        AX12_ANGLE_BRAS_BASE_DROIT_REPLIER,
+        AX12_SPEED_BRAS_BASE
+    );
+    AX12_setGoal(
+        AX12_ID_BRAS_BASE_GAUCHE,
+        AX12_ANGLE_BRAS_BASE_GAUCHE_REPLIER,
+        AX12_SPEED_BRAS_RELACHEUR
+    );
+    AX12_setGoal(
+        AX12_ID_PALET_DROIT,
+        AX12_ANGLE_PALET_DROIT_FERMER,
+        AX12_SPEED_PALET
+    );
+    AX12_setGoal(
+        AX12_ID_PALET_GAUCHE,
+        AX12_ANGLE_PALET_GAUCHE_FERMER,
+        AX12_SPEED_PALET
+    );
+    AX12_processChange();
+}
+
+/****************************************************************************************/
 /* FUNCTION NAME: runTest                                                               */
 /* DESCRIPTION  : tester l'ensemble des actionneurs du robot                            */
 /****************************************************************************************/
 void runRobotTest(void) 
 {
+    XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_SECURISE);
+    XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_SECURISE);
     
+    wait_ms(200);
+    
+    XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_FERMER);
+    XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_FERMER);
+    
+    wait_ms(200);
+    
+    AX12_setGoal(
+        AX12_ID_BRAS_BASE_DROIT,
+        AX12_ANGLE_BRAS_BASE_DROIT_MOITER,
+        AX12_SPEED_BRAS_BASE
+    );
+    AX12_setGoal(
+        AX12_ID_BRAS_BASE_GAUCHE,
+        AX12_ANGLE_BRAS_BASE_GAUCHE_MOITER,
+        AX12_SPEED_BRAS_BASE
+    );
+    AX12_processChange();
+        
+    wait_ms(500);        
+    
+    AX12_setGoal(
+        AX12_ID_BRAS_RELACHEUR_DROIT,
+        AX12_ANGLE_BRAS_RELACHEUR_DROIT_OUVERT,
+        AX12_SPEED_BRAS_RELACHEUR
+    );
+    AX12_setGoal(
+        AX12_ID_BRAS_RELACHEUR_GAUCHE,
+        AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_OUVERT,
+        AX12_SPEED_BRAS_RELACHEUR
+    );
+    AX12_processChange();
+    
+    wait_ms(600);
+    
+    AX12_setGoal(
+        AX12_ID_BRAS_RELACHEUR_DROIT,
+        AX12_ANGLE_BRAS_RELACHEUR_DROIT_REPLIER,
+        AX12_SPEED_BRAS_RELACHEUR
+    );
+    AX12_setGoal(
+        AX12_ID_BRAS_RELACHEUR_GAUCHE,
+        AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_REPLIER,
+        AX12_SPEED_BRAS_RELACHEUR
+    );
+    AX12_processChange();
+    
+    wait_ms(500);
+    
+    AX12_setGoal(
+        AX12_ID_BRAS_BASE_DROIT,
+        AX12_ANGLE_BRAS_BASE_DROIT_REPLIER,
+        AX12_SPEED_BRAS_BASE
+    );
+    AX12_setGoal(
+        AX12_ID_BRAS_BASE_GAUCHE,
+        AX12_ANGLE_BRAS_BASE_GAUCHE_REPLIER,
+        AX12_SPEED_BRAS_RELACHEUR
+    );
+    AX12_processChange();
+    
+    wait_ms(600);
+    AX12_setGoal(
+        AX12_ID_PALET_DROIT,
+        AX12_ANGLE_PALET_DROIT_ROCHET,
+        AX12_SPEED_PALET
+    );
+    AX12_setGoal(
+        AX12_ID_PALET_GAUCHE,
+        AX12_ANGLE_PALET_GAUCHE_ROCHET,
+        AX12_SPEED_PALET
+    );
+    AX12_processChange();
+    
+    wait_ms(500);
+    
+    AX12_setGoal(
+        AX12_ID_PALET_DROIT,
+        AX12_ANGLE_PALET_DROIT_FERMER,
+        AX12_SPEED_PALET
+    );
+    AX12_setGoal(
+        AX12_ID_PALET_GAUCHE,
+        AX12_ANGLE_PALET_GAUCHE_FERMER,
+        AX12_SPEED_PALET
+    );
+    AX12_processChange();
 }
 
 /****************************************************************************************/
@@ -304,6 +466,8 @@
 /****************************************************************************************/
 int SelectStrategy(unsigned char id)
 {
+    
+    
     switch(id)
     {
         case 1:
@@ -321,10 +485,34 @@
         case 5:
             strcpy(cheminFileStart,"/local/strat5.txt");
             return FileExists(cheminFileStart);
+        case 6:
+            strcpy(cheminFileStart,"/local/strat6.txt");
+            return FileExists(cheminFileStart);
+        case 10:
+            strcpy(cheminFileStart,"/local/grand_8.txt");
+            return FileExists(cheminFileStart);
         default:
-            strcpy(cheminFileStart,"/local/strat.txt");
+            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)
+{
+    
+}
+
 #endif

--- a/Strategie/Strategie.cpp	Mon May 02 19:40:59 2016 +0000
+++ b/Strategie/Strategie.cpp	Mon May 09 09:10:17 2016 +0000
@@ -18,10 +18,12 @@
 
 signed short x_robot,y_robot,theta_robot;//La position du robot
 
+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
+
 #ifdef ROBOT_BIG
-unsigned short id_check[NOMBRE_CARTES]= {CHECK_BALISE,CHECK_MOTEUR,CHECK_ACTIONNEURS};
-unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_BALISE,ALIVE_MOTEUR,ALIVE_ACTIONNEURS};
-InterruptIn jack(p24); //  entrée analogique en interruption pour le jack
+unsigned short id_check[NOMBRE_CARTES]= {CHECK_BALISE,CHECK_MOTEUR,CHECK_ACTIONNEURS,CHECK_AX12,CHECK_POMPES};
+unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_BALISE,ALIVE_MOTEUR,ALIVE_ACTIONNEURS,ALIVE_AX12,ALIVE_POMPES};
+InterruptIn jack(p25); //  entrée analogique en interruption pour le jack
 #else
 unsigned short id_check[NOMBRE_CARTES]= {CHECK_BALISE,CHECK_MOTEUR,CHECK_ACTIONNEURS};
 unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_BALISE,ALIVE_MOTEUR,ALIVE_ACTIONNEURS};
@@ -34,6 +36,8 @@
 
 unsigned char countRobotNear = 0;//Le nombre de robot à proximité
 
+unsigned char ingnorBaliseOnce = 0;
+
 /****************************************************************************************/
 /* FUNCTION NAME: chronometre_ISR                                                       */
 /* DESCRIPTION  : Interruption à la fin des 90s du match                                */
@@ -45,6 +49,7 @@
     gameTimer.stop();//Arret du timer
 
 #ifdef ROBOT_BIG
+    wait_ms(2000);
     doFunnyAction();
 #endif
     
@@ -78,7 +83,7 @@
     signed short localData4 = 0;
     unsigned char localData5 = 0;
     
-    if(gameTimer.read_ms() >= 88000) {//Fin du match (On autorise 2s pour déposer des éléments
+    if(gameTimer.read_ms() >= 89000) {//Fin du match (On autorise 2s pour déposer des éléments
         gameTimer.stop();
         gameTimer.reset();
         gameEtat = ETAT_END;//Fin du temps
@@ -149,6 +154,7 @@
                 countAliveCard++;
                 checkCurrent++;
                 if(checkCurrent >= NOMBRE_CARTES) {
+                    //printf("all card check, missing %d cards\n",(NOMBRE_CARTES-countAliveCard));
                     if(countAliveCard >= NOMBRE_CARTES) {
                         gameEtat = ETAT_CONFIG;
                         SendRawId(ECRAN_ALL_CHECK);//On dit à l'IHM que toutes les cartes sont en ligne
@@ -163,8 +169,10 @@
             } else if(cartesCheker.read_ms () > 100) {
                 cartesCheker.stop();
                 if(screenChecktry >=3) {
+                    //printf("missing card %d\n",id_check[checkCurrent]);
                     screenChecktry = 0;
                     checkCurrent++;
+                    
                     if(checkCurrent >= NOMBRE_CARTES) {
                         if(countAliveCard == NOMBRE_CARTES) {
                             gameEtat = ETAT_CONFIG;
@@ -198,29 +206,37 @@
         break;
         case ETAT_GAME_INIT:
             //On charge la liste des instructions
-            //strcpy(cheminFileStart,"/local/test.txt");//On ouvre le fichier test.txt
             loadAllInstruction();//Mise en cache de toute les instructions
             gameEtat = ETAT_GAME_WAIT_FOR_JACK;
             SendRawId(ECRAN_ACK_START_MATCH);
             tactile_printf("Attente du JACK.");
+            setAsservissementEtat(1);//On réactive l'asservissement
+            jack.mode(PullDown); // désactivation de la résistance interne du jack
             jack.fall(&jack_ISR); // création de l'interrupt attachée au changement d'état (front descendant) sur le jack
-     /*       
-#ifdef ROBOT_BIG
-            SetOdometrie(ODOMETRIE_BIG_POSITION, POSITION_DEBUT_X,POSITION_DEBUT_Y,POSITION_DEBUT_T);
-#else
+         
+#ifdef ROBOT_BIG //le gros robot n'a pas de recalage bordure pour ce placer au début, on lui envoit donc ça position
+            localData2 = POSITION_DEBUT_T;
+            localData3 = POSITION_DEBUT_Y;
+            if(InversStrat == 1) {
+                localData2 = -localData2;//Inversion theta
+                localData3 = 3000 - POSITION_DEBUT_Y;//Inversion du Y
+            }
+            SetOdometrie(ODOMETRIE_BIG_POSITION, POSITION_DEBUT_X,localData3,localData2);
+#endif /*
             SetOdometrie(ODOMETRIE_SMALL_POSITION, POSITION_DEBUT_X,POSITION_DEBUT_Y,POSITION_DEBUT_T);
 #endif*/
         break;
         case ETAT_GAME_WAIT_FOR_JACK:
-            //TODO Attendre le jack
+            //On attend le jack
         break;
         case ETAT_GAME_START:
-            chronoEnd.attach(&chronometre_ISR,90);
+            chronoEnd.attach(&chronometre_ISR,90);//On lance le chrono de 90s
             gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
             gameTimer.reset();
             gameTimer.start();
-            jack.fall(NULL);
+            jack.fall(NULL);//On désactive l'interruption du jack
             SendRawId(GLOBAL_START);
+            tactile_printf("Start");//Pas vraiment util mais bon
         break;
         case ETAT_GAME_LOAD_NEXT_INSTRUCTION:
             /*
@@ -234,10 +250,8 @@
                 instruction = strat_instructions[actual_instruction];
                 //On effectue le traitement de l'instruction 
                 gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;
-                //actual_instruction++;
             }
             screenChecktry = 0;
-            wait_ms(100);
         break;
         case ETAT_GAME_PROCESS_INSTRUCTION:
             /*
@@ -246,7 +260,7 @@
             //debug_Instruction(instruction);
             switch(instruction.order)
             {
-                case MV_COURBURE:
+                case MV_COURBURE://C'est un rayon de courbure
                     waitingAckID = ASSERVISSEMENT_COURBURE;
                     waitingAckFrom = ACKNOWLEDGE_MOTEUR;
                     if(instruction.nextActionType == ENCHAINEMENT) {
@@ -261,16 +275,20 @@
                         }
                     }
                     localData1 = ((instruction.direction == LEFT)?1:-1);
+                    if(InversStrat == 1)
+                    {
+                        localData1 = -localData1;//Inversion de la direction
+                    }
                     BendRadius(instruction.arg1, instruction.arg3, localData1, localData5);
                 break;
-                case MV_LINE:
+                case MV_LINE://Ligne droite
                     waitingAckID = ASSERVISSEMENT_RECALAGE;
                     waitingAckFrom = ACKNOWLEDGE_MOTEUR;
                     if(instruction.nextActionType == ENCHAINEMENT) {
                         MV_enchainement++;
                         localData5 = 1;
                     } else {
-                        if(MV_enchainement > 0) {
+                        if(MV_enchainement > 0) {//Utilisé en cas d'enchainement, 
                             localData5 = 2;
                             MV_enchainement = 0;
                         } else {
@@ -280,19 +298,23 @@
                     localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1);
                     GoStraight(localData2, 0, 0, localData5);
                 break;
-                case MV_TURN:
+                case MV_TURN: //Rotation sur place
                     if(instruction.direction == RELATIVE) {
                         localData2 = instruction.arg3;
-                    } else {
-                        if(abs(instruction.arg3 - theta_robot) > 180) {
-                            localData2 = 360 - instruction.arg3 - theta_robot;
-                        } else {
-                            localData2 = instruction.arg3 - theta_robot;
+                    } else {//C'est un rotation absolu, il faut la convertir en relative
+                        localData2 = instruction.arg3;
+                        
+                        if(InversStrat == 1) {
+                            localData2 = -localData2;
                         }
+                        
+                        localData2 = (localData2 - theta_robot)%3600;
+                        if(localData2 > 1800) {
+                            localData2 = localData2-3600;
+                        }
+                        
                     }
-                    if(InversStrat == 1) {
-                        localData2 = -localData2;
-                    }
+                    
                     Rotate(localData2);
                     waitingAckID = ASSERVISSEMENT_ROTATION;
                     waitingAckFrom = ACKNOWLEDGE_MOTEUR;
@@ -305,12 +327,13 @@
                     }
                     
                     if(InversStrat == 1) {
-                        //localData2 = 360 - instruction.arg3
+                        localData2 = -instruction.arg3;
                         localData3 = 3000 - instruction.arg2;//Inversion du Y
                     } else {
                         localData3 = instruction.arg2;
+                        localData2 = instruction.arg3;
                     }
-                    GoToPosition(instruction.arg1,localData3,instruction.arg3,localData1);
+                    GoToPosition(instruction.arg1,localData3,localData2,localData1);
                     waitingAckID = ASSERVISSEMENT_XYT;
                     waitingAckFrom = ACKNOWLEDGE_MOTEUR;
                 break;
@@ -318,7 +341,7 @@
                     waitingAckID = ASSERVISSEMENT_RECALAGE;
                     waitingAckFrom = ACKNOWLEDGE_MOTEUR;
                     instruction.nextActionType = WAIT;
-                    localData2 = (((instruction.direction == FORWARD)?1:-1)*3000);
+                    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;
@@ -454,20 +477,30 @@
         
         
         case ETAT_WARNING_TIMEOUT://Attente de la trame fin de danger ou du timeout de 2s
-            if(timeoutWarning.read_ms() >= 2000)//ça fait plus de 2s, il faut changer de stratégie
+            if(timeoutWarning.read_ms() >= BALISE_TIMEOUT)//ça fait plus de 2s, il faut changer de stratégie
             {
                 gameEtat = ETAT_WARNING_SWITCH_STRATEGIE;
             }
         break;
         case ETAT_WARING_END_BALISE_WAIT://Attente d'une seconde apres la fin d'un End Balise pour etre sur que c'est bon
-            
+            if(timeoutWarningWaitEnd.read_ms() >= 1000) {//c'est bon, on repart
+                //actual_instruction = instruction.nextLineError;
+                gameEtat = ETAT_WARNING_END_LAST_INSTRUCTION;
+            }
         break;
         case ETAT_WARNING_END_LAST_INSTRUCTION://trouver le meilleur moyen de reprendre l'instruction en cours
-            
+#ifdef ROBOT_BIG
+            actual_instruction = 2;//Modification directe... c'est pas bien mais ça marchait pour le match 5
+            gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
+#else
+            actual_instruction = instruction.nextLineError;
+            gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; 
+#endif
         break;
         case ETAT_WARNING_SWITCH_STRATEGIE://Si à la fin du timeout il y a toujours un robot, passer à l'instruction d'erreur
             actual_instruction = instruction.nextLineError;
             gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
+            ingnorBaliseOnce = 1;
         break;
         
         
@@ -512,6 +545,7 @@
             case ALIVE_IHM:
             case ALIVE_ACTIONNEURS:
             case ALIVE_POMPES:
+            case ALIVE_AX12:
             case ECRAN_ALL_CHECK:
                 if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id) {
                     waitingAckFrom = 0;//C'est la bonne carte qui indique qu'elle est en ligne
@@ -590,19 +624,26 @@
                     }
                     can1.write(msgTx);
                     wait_ms(10);
-                    tactile_printf("Strat %d selectionne",msgTx.data[0]);
+                    setAsservissementEtat(0);//Désactivation de l'asservissement pour repositionner le robot dans le zone de départ
+                    tactile_printf("Strat %d, Asser desactive",msgTx.data[0]);
                 }
             break;
             case BALISE_STOP:
                 SendAck(ACKNOWLEDGE_BALISE, BALISE_STOP);
-                if(gameEtat > ETAT_GAME_START && gameEtat != ETAT_WARNING_TIMEOUT)
-                {
-                    wait_ms(10);
-                    SendRawId(ASSERVISSEMENT_STOP);
-                    gameEtat = ETAT_WARNING_TIMEOUT;
-                    timeoutWarning.reset();
-                    timeoutWarning.start();//Reset du timer utiliser par le timeout
+                
+                if(needToStop() != 0 && ingnorBaliseOnce ==0) {
+                    if(gameEtat > ETAT_GAME_START && gameEtat != ETAT_WARNING_TIMEOUT)
+                    {
+                        SendRawId(ASSERVISSEMENT_STOP);
+                        //while(1);
+                        gameEtat = ETAT_WARNING_TIMEOUT;
+                        if(gameEtat != ETAT_WARING_END_BALISE_WAIT) {
+                            timeoutWarning.reset();
+                            timeoutWarning.start();//Reset du timer utiliser par le timeout
+                        }
+                    }
                 }
+                ingnorBaliseOnce = 0;
             break;
             case BALISE_END_DANGER:
                 if(gameEtat == ETAT_WARNING_TIMEOUT) {
@@ -611,6 +652,18 @@
                     gameEtat = ETAT_WARING_END_BALISE_WAIT;
                 }
             break;
+            
+            case ECRAN_CHOICE_START_ACTION:
+                if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config
+                    if(msgRxBuffer[FIFO_lecture].data[0] == 1) {
+                        runRobotTest();
+                    } else {
+                        initRobotActionneur();
+                    }
+                    wait_ms(500);
+                    SendRawId(ECRAN_ACK_CHOICE_START_ACTION);
+                }
+            break;
         }        
         
         FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;

--- a/main.cpp	Mon May 02 19:40:59 2016 +0000
+++ b/main.cpp	Mon May 09 09:10:17 2016 +0000
@@ -33,10 +33,14 @@
     can1.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN
     
     wait_ms(1000);
-    tactile_printf("Initialisation cartes...");
+#ifdef ROBOT_BIG
+    tactile_printf("Initialisation gros robot");
+#else
+    tactile_printf("Initialisation petit robot");
+#endif
     initRobot();//Initialisation du robot
     
-    wait_ms(5000);//Attente pour que toutes les cartes se lancent et surtout le CANBlue
+    wait_ms(1000);//Attente pour que toutes les cartes se lancent et surtout le CANBlue
     
     /**
     A retirer lors de l'utilisation avec selecteur de stratégie sur IHM