Secoue les planches, v1.2

Actionneurs/Actionneurs.ino

@@ -5,28 +5,26 @@
 #define SERVO_PINCE_GAUCHE 8
 #define SERVO_PINCE_DROITE 9
 
-#define FOURCHE_TRANSPORT 5, 287
-#define FOURCHE_LEVEE     5, 217
-#define FOURCHE_PRISE     5, 327
+#define FOURCHE_TRANSPORT 5, 277
+#define FOURCHE_LEVEE     5, 206
+#define FOURCHE_PRISE     5, 308
 
-#define DOIGT_PINCE_GAUCHE_FERME 3, 1500
-#define DOIGT_PINCE_GAUCHE_OUVRE 3, 1842
-#define DOIGT_PINCE_GAUCHE_SEUIL 1515
+#define DOIGT_PINCE_GAUCHE_FERME 3, 1530
+#define DOIGT_PINCE_GAUCHE_OUVRE 3, 1790
+#define DOIGT_PINCE_GAUCHE_SEUIL 1545
 
-/// TODO renseigner de vraies valeurs
-#define DOIGT_PINCE_DROIT_FERME 4, 1000
-#define DOIGT_PINCE_DROIT_OUVRE 4, 800
-#define DOIGT_PINCE_DROIT_SEUIL 815
-/// FIN TODO
+#define DOIGT_PINCE_DROIT_FERME 4, 2500
+#define DOIGT_PINCE_DROIT_OUVRE 4, 2365
+#define DOIGT_PINCE_DROIT_SEUIL 1575
 
-#define AIMANT_PINCE_DROIT_TIENT 8, 147
-#define AIMANT_PINCE_DROIT_LACHE 8, 337
+#define AIMANT_PINCE_DROIT_TIENT 8, 176
+#define AIMANT_PINCE_DROIT_LACHE 8, 366
 
-#define AIMANT_PINCE_GAUCHE_TIENT 9, 500
-#define AIMANT_PINCE_GAUCHE_LACHE 9, 400
+#define AIMANT_PINCE_GAUCHE_TIENT 9, 366
+#define AIMANT_PINCE_GAUCHE_LACHE 9, 236
 
 #define I2C_CDE_DEMANDE 0x00
-#define I2C_DCE_REALISE 0x01
+#define I2C_CDE_REALISE 0x01
 
 volatile byte * I2C_memory;
 
@@ -34,14 +32,10 @@ SMS_STS sms_sts;
 
 int ID_Feetech = 4;
 
-struct position_t{
-  int nb_tour;
-  uint position;
-} cible_haute, cible_basse, actuelle;
-
 enum etat_action_t Actionneur_deplacement(void);
 enum etat_action_t Actionneur_prise_initiale(void);
 enum etat_action_t Actionneur_empile(void);
+enum etat_action_t Actionneur_deplacement(void);
 
 void setup()
 {
@@ -49,15 +43,8 @@ void setup()
   Serial.begin(115200);//sms舵机波特率115200
   Serial1.begin(1000000,SERIAL_8N1, RX, TX);//sts舵机波特率1000000
   sms_sts.pSerial = &Serial1;
-  delay(5000);
+  //delay(5000);
   
-  cible_haute.nb_tour = 0;
-  cible_haute.position = 500;
-
-  cible_basse.nb_tour = 2;
-  cible_basse.position = 1500;
-
-  actuelle.nb_tour = 0;
 
   //Servo fourche (12V)
   ledcAttach(5, 50, 12);
@@ -94,7 +81,6 @@ void setup()
     }
   }while(error);
 
-
   // Servo pinces à aimant
   ledcAttach(8, 50, 12);
   ledcWrite(8, 307);
@@ -104,6 +90,7 @@ void setup()
   
 
   Translateur_init();
+
   
 
   /*while(1){
@@ -127,28 +114,8 @@ void setup()
     delay(5000);
 
   }*/
-  /*
-  sms_sts.WheelMode(10);
-  sms_sts.WriteSpe(10, 2000, 50);
-  while(1){
-    printf(">load_d:%d\n", sms_sts.ReadLoad(10));
-    delay(25);
-  }*/
-
-
-
-
-  if(sms_sts.FeedBack(ID_Feetech)!=-1){
-    actuelle.position = sms_sts.ReadPos(-1);
-  }else{
-    do{
-      Serial.println("Erreur lecture");
-      delay(1000);
-    }while(sms_sts.FeedBack(ID_Feetech) ==-1);
-    actuelle.position = sms_sts.ReadPos(-1);
-  }
-
   Ascenseur_init();  
+
 }
 
 void reglage_servo(int pin_servo){
@@ -196,15 +163,12 @@ void loop()
 {
   static int m_pos=0;
   static unsigned long myTime=0;
-  static position_t cible;
-
   
-  
-  
-  if(millis() > myTime + 100){
+  if(millis() > myTime + 30){
     myTime = millis();
     Serial.print(">millis:");
     Serial.println(millis());
+    Serial.printf(">I2C_c:%d\n>I2C_t:%d\n", I2C_memory[I2C_CDE_DEMANDE], I2C_memory[I2C_CDE_REALISE]);
   }
 
 
@@ -216,11 +180,12 @@ void loop()
   int Move;
   int Current;
 
-  if(I2C_memory[I2C_CDE_DEMANDE] != I2C_memory[I2C_DCE_REALISE]){
+  if(I2C_memory[I2C_CDE_DEMANDE] != I2C_memory[I2C_CDE_REALISE]){
+    
     switch (I2C_memory[I2C_CDE_DEMANDE]){
       case 0:
         // Aucune commande
-        I2C_memory[I2C_DCE_REALISE] = 0;
+        I2C_memory[I2C_CDE_REALISE] = 0;
         break;
 
       case 1:
@@ -231,29 +196,46 @@ void loop()
         // Position de déplacement
         // Fourche levée pour ne pas géner les capteurs
         if(Actionneur_deplacement() == ACTION_TERMINEE){
-          I2C_memory[I2C_DCE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
+          I2C_memory[I2C_CDE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
         }
         break;
         
       case 3:
-        // Prise initiale
-        if(Actionneur_prise_initiale() == ACTION_TERMINEE){
-          I2C_memory[I2C_DCE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
+        // Position de prise planche
+        if(Actionneur_prepare_prise_planche() == ACTION_TERMINEE){
+          I2C_memory[I2C_CDE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
         }
         break;
 
       case 4:
-        // Position de transport
-        // Utile ? - n'est-ce pas la position après la prise initiale ?
+        // Prise initiale 
+        if(Actionneur_prise_initiale() == ACTION_TERMINEE){
+          I2C_memory[I2C_CDE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
+        }
         break;
 
       case 5:
         // Empile
         if(Actionneur_empile() == ACTION_TERMINEE){
-          I2C_memory[I2C_DCE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
+          Serial.println("I2C : ACTION_TERMINEE !!!");
+          I2C_memory[I2C_CDE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
         }
         break;
+
+      case 6:
+        while(Actionneur_depile_planche() != ACTION_TERMINEE);
+        I2C_memory[I2C_CDE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
+        break;
       
+      case 7:
+        while(Actionneur_prepare_prise_extern() != ACTION_TERMINEE);
+        I2C_memory[I2C_CDE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
+        break;
+
+      case 8:
+        while(Actionneur_fourche_transport() != ACTION_TERMINEE);
+        I2C_memory[I2C_CDE_REALISE] = I2C_memory[I2C_CDE_DEMANDE];
+        break;      
 
     }
 
@@ -280,10 +262,12 @@ void loop()
       Serial.println("v: Cycle ascenseur");
       Serial.println("c: Cycle translateur");
       Serial.println("h: Actionneur, position de déplacement");
+      Serial.println("n: Actionneur, prepare prise");
       Serial.println("j: Actionneur, prise initiale");
       Serial.println("k: Actionneur, empile");
       
       
+      
     }
     if(inByte == 'd'){
       Ascenseur_step_down();
@@ -305,9 +289,17 @@ void loop()
     }
     if(inByte == 'w'){
       Servo_set(DOIGT_PINCE_GAUCHE_OUVRE);
+      Servo_set(DOIGT_PINCE_DROIT_OUVRE);
     }
     if(inByte == 'x'){
       Servo_set(DOIGT_PINCE_GAUCHE_FERME);
+      Servo_set(DOIGT_PINCE_DROIT_FERME);
+    }
+    if(inByte == 'f'){
+      Servo_set(FOURCHE_LEVEE);
+    }
+    if(inByte == 'y'){
+      while(Actionneur_depile_planche() == ACTION_EN_COURS);
     }
     if(inByte == '1'){
       reglage_servo(8);
@@ -327,6 +319,9 @@ void loop()
     if(inByte == 'k'){
       while(Actionneur_empile() == ACTION_EN_COURS);
     }
+    if(inByte == 'n'){
+      while(Actionneur_prepare_prise_planche() == ACTION_EN_COURS);
+    }
     /*
     if(inByte == 'u'){
       while(Actionneur_deplacement() == ACTION_EN_COURS);
@@ -365,34 +360,6 @@ void loop()
   Serial.printf(">OUT4:%d\n",digitalRead(4));
 
   Ascenseur_gestion();
-  /*
-  if(sms_sts.FeedBack(ID)!=-1){
-    Pos = sms_sts.ReadPos(-1);
-    Speed = sms_sts.ReadSpeed(-1);
-    Load = sms_sts.ReadLoad(-1);
-    Voltage = sms_sts.ReadVoltage(-1);
-    Temper = sms_sts.ReadTemper(-1);
-    Move = sms_sts.ReadMove(-1);
-    Current = sms_sts.ReadCurrent(-1);
-    Serial.print(">Position:");
-    Serial.println(Pos);
-    Serial.print(">Speed:");
-    Serial.println(Speed);
-    Serial.print(">Load:");
-    Serial.println(Load);
-    Serial.print(">Voltage:");
-    Serial.println(Voltage);
-    Serial.print(">Temper:");
-    Serial.println(Temper);
-    Serial.print(">Move:");
-    Serial.println(Move);
-    Serial.print(">Current:");
-    Serial.println(Current);
-    delay(50);
-  }else{
-    Serial.println("FeedBack err");
-    delay(500);
-  }*/
 
   delay(25);
 }
@@ -430,20 +397,66 @@ enum etat_action_t Actionneur_deplacement(void){
 
 }
 
+enum etat_action_t Actionneur_prepare_prise_extern(void){
+  Servo_set(FOURCHE_TRANSPORT);
+  // Ascenseur en bas
+  Ascenseur_descend();
+  // Pinces internes fermées
+  Servo_set(DOIGT_PINCE_DROIT_FERME);
+  Servo_set(DOIGT_PINCE_GAUCHE_FERME);
+  // Aimant sortis
+  Servo_set(AIMANT_PINCE_DROIT_LACHE);
+  Servo_set(AIMANT_PINCE_GAUCHE_LACHE);
+  // Translateur à l'arrière
+  Translateur_recule();
+
+  return ACTION_TERMINEE;
+
+}
+
+enum etat_action_t Actionneur_prepare_prise_planche(void){
+  Servo_set(FOURCHE_PRISE);
+  // Ascenseur en bas
+  Ascenseur_descend();
+  // Pinces internes fermées
+  Servo_set(DOIGT_PINCE_DROIT_FERME);
+  Servo_set(DOIGT_PINCE_GAUCHE_FERME);
+  // Aimant rentrés
+  Servo_set(AIMANT_PINCE_DROIT_TIENT);
+  Servo_set(AIMANT_PINCE_GAUCHE_TIENT);
+  // Translateur à l'arrière
+  Translateur_recule();
+
+  return ACTION_TERMINEE;
+
+}
+
+enum etat_action_t Actionneur_depile_planche(void){
+  Servo_set(AIMANT_PINCE_DROIT_LACHE);
+  Servo_set(AIMANT_PINCE_GAUCHE_LACHE);
+  delay(1000);
+  ledcWrite(FOURCHE_LEVEE);
+  delay(1000);
+  for(int i=0;i<20;i++){
+    ledcWrite(FOURCHE_PRISE);
+    delay(50);
+    ledcWrite(FOURCHE_LEVEE);
+    delay(200);
+  }
+  
+  return ACTION_TERMINEE;
+
+}
+
 enum etat_action_t Actionneur_prise_initiale(void){
   static enum etat_actionneur_t{
-    ACTIONNEUR_DEPILE,
     ACTIONNEUR_PRISE_INTERNE_1,
     ACTIONNEUR_PRISE_INTERNE_2,
     ACTIONNEUR_RECULE_PRISE_INTERNE_1,
     ACTIONNEUR_RECULE_PRISE_INTERNE_2
-  } etat_actionneur=ACTIONNEUR_DEPILE;
+  } etat_actionneur=ACTIONNEUR_PRISE_INTERNE_1;
 
   switch(etat_actionneur){
-    case ACTIONNEUR_DEPILE:
-      Servo_set(FOURCHE_LEVEE);
-      etat_actionneur = ACTIONNEUR_PRISE_INTERNE_1;
-      break;
 
     case ACTIONNEUR_PRISE_INTERNE_1:
       Serial.println("ACTIONNEUR_PRISE_INTERNE_1");
@@ -474,8 +487,8 @@ enum etat_action_t Actionneur_prise_initiale(void){
     case ACTIONNEUR_RECULE_PRISE_INTERNE_2:
       Serial.println("ACTIONNEUR_RECULE_PRISE_INTERNE_2");
       if(Translateur_etat() == ACTION_TERMINEE){
-        Servo_set(FOURCHE_TRANSPORT);
-        etat_actionneur=ACTIONNEUR_DEPILE;
+        //Servo_set(FOURCHE_TRANSPORT);
+        etat_actionneur=ACTIONNEUR_PRISE_INTERNE_1;
         return ACTION_TERMINEE;
       }
       break;
@@ -485,6 +498,11 @@ enum etat_action_t Actionneur_prise_initiale(void){
 
 }
 
+enum etat_action_t Actionneur_fourche_transport(){
+  Servo_set(FOURCHE_TRANSPORT);
+  return ACTION_TERMINEE;
+}
+
 enum etat_action_t Actionneur_empile(){
   static enum etat_actionneur_t{
     ACTIONNEUR_LEVE,
@@ -499,6 +517,7 @@ enum etat_action_t Actionneur_empile(){
     case ACTIONNEUR_LEVE:
       Serial.println("ACTIONNEUR_LEVE");
       etat_actionneur = ACTIONNEUR_LEVE_1;
+      Servo_set(FOURCHE_PRISE);
       Ascenseur_monte();
       break;
 
@@ -536,6 +555,8 @@ enum etat_action_t Actionneur_empile(){
         etat_actionneur = ACTIONNEUR_DESCEND;
         Servo_set(DOIGT_PINCE_DROIT_FERME);
         Servo_set(DOIGT_PINCE_GAUCHE_FERME);
+        Servo_set(AIMANT_PINCE_DROIT_LACHE);
+        Servo_set(AIMANT_PINCE_GAUCHE_LACHE);
         Ascenseur_descend();
       }
       break;

Actionneurs/Ascenseur.ino

@@ -86,7 +86,7 @@ enum etat_action_t Ascenseur_cherche_butees(void){
       if(abs(pos_d - old_pos_d) < 10 ){*/
       my_load = sms_sts.ReadLoad(ID_FEETECH_ASC_D);
       printf(">load_d:%d\n", my_load);
-      if(abs(my_load) > 250 ){
+      if(abs(my_load) > 200 ){
         sms_sts.EnableTorque(ID_FEETECH_ASC_D, 0);
         position_basse_droit = sms_sts.ReadPos(ID_FEETECH_ASC_D);
         Serial.printf("position_basse_droit:%d\n", position_basse_droit);
@@ -99,7 +99,7 @@ enum etat_action_t Ascenseur_cherche_butees(void){
       printf(">delta_pos_g:%d\n",pos_g - old_pos_g);*/
       my_load = sms_sts.ReadLoad(ID_FEETECH_ASC_G);
       printf(">load_g:%d\n", my_load);
-      if(abs(my_load) > 250 ){
+      if(abs(my_load) > 200 ){
         sms_sts.EnableTorque(ID_FEETECH_ASC_G, 0);
         position_basse_gauche = sms_sts.ReadPos(ID_FEETECH_ASC_G);
         Serial.printf("position_basse_gauche:%d\n", position_basse_gauche);
@@ -120,8 +120,8 @@ enum etat_action_t Ascenseur_cherche_butees(void){
       sms_sts.ServoMode(ID_FEETECH_ASC_G);
       tab_position[0] = position_basse_droit + (-250 * ASC_D_SIGNE); // Droit
       tab_position[1] = position_basse_gauche + (-250 * ASC_G_SIGNE); // Gauche
-      position_haute_droit = position_basse_droit - (8100 * ASC_D_SIGNE);
-      position_haute_gauche = position_basse_gauche - (8100 * ASC_G_SIGNE);
+      position_haute_droit = position_basse_droit - (9400 * ASC_D_SIGNE);
+      position_haute_gauche = position_basse_gauche - (9400 * ASC_G_SIGNE);
       tab_vitesses_u[0] = 2000;
       tab_vitesses_u[1] = 2000;
       sms_sts.SyncWritePosEx(asc_ID, 2, tab_position, tab_vitesses_u, tab_acc);
@@ -169,8 +169,8 @@ int Ascenseur_descend(void){
 /// @return 1 si l'ascenseur n'est pas prêt
 int Ascenseur_depose(void){
   if(etat_ascenseur == ASCENSEUR_ACTIF){
-    tab_position[0] = position_haute_droit*0.9 + position_basse_droit * 0.1;
-    tab_position[1] = position_haute_gauche * 0.9 + position_basse_gauche * 0.1;
+    tab_position[0] = position_haute_droit*0.8 + position_basse_droit * 0.2;
+    tab_position[1] = position_haute_gauche * 0.8 + position_basse_gauche * 0.2;
     tab_vitesses_u[0] = 2000;
     tab_vitesses_u[1] = 2000;
     sms_sts.SyncWritePosEx(asc_ID, 2, tab_position, tab_vitesses_u, tab_acc);
@@ -207,13 +207,13 @@ void Ascenseur_update_step(void){
   }
   if((float)(tab_position[0] - position_basse_droit) /(float)(position_haute_droit - position_basse_droit) < 0){
     tab_position[0] = position_basse_droit;
-  }
+  }/*
   if((float)(tab_position[1] - position_basse_gauche) /(float)(position_haute_gauche - position_basse_gauche) > 1){
     tab_position[1] = position_haute_gauche;
   }
   if((float)(tab_position[1] - position_basse_gauche) /(float)(position_haute_gauche - position_basse_gauche) < 0){
     tab_position[1] = position_basse_gauche;
-  }
+  }*/
   tab_vitesses_u[0] = 2000;
   tab_vitesses_u[1] = 2000;
   sms_sts.SyncWritePosEx(asc_ID, 2, tab_position, tab_vitesses_u, tab_acc);

Cerveau/Cerveau.ino

@@ -6,6 +6,7 @@
 #include "Com_chassis.h"
 #include "Com_detection_adversaire.h"
 #include "Com_gradins.h"
+#include "Com_actionneur.h"
 #include "ServerWeb.h"
 
 
@@ -280,6 +281,7 @@ void gestion_match(){
   struct detect_gradin_t detect_gradin;
   enum etat_action_t etat_action;
   static int translation_x_mm, translation_y_mm;
+  static char tirette_enlevee=1;
   static float rotation_rad;
   static int couleur;
 
@@ -319,6 +321,7 @@ void gestion_match(){
         translation_x_mm = 500;
         translation_y_mm = 0;
         rotation_rad = 0;
+        //index_Maitre = DEPLACEMENT_RELATIF;
         
         index_Maitre = TEST_APPROCHE_GRADIN;      
       }
@@ -335,12 +338,17 @@ void gestion_match(){
         translation_x_mm = 0;
         translation_y_mm = 0;
         rotation_rad = 100;
+        Serial.println("C1");
         Triangulation_send_immobile(1);
+        Serial.println("C2");
         while(M5.BtnC.read()){
           M5.update();
         }
+        Serial.println("C3");
         delay(200);
+        Serial.println("C4");
         IHM_attente_match(&couleur);
+        tirette_enlevee = 0;
 
         index_Maitre = MATCH_EN_COURS;
         
@@ -361,14 +369,20 @@ void gestion_match(){
 
     case DEPLACEMENT_RELATIF :    // Deplacement relatif en cours
 
-      if(deplacement_relatif(translation_x_mm, translation_y_mm, rotation_rad, 1) == ACTION_TERMINEE){
+      if(deplacement_relatif(translation_x_mm, translation_y_mm, rotation_rad, 1) != ACTION_EN_COURS){
         index_Maitre = ATTENTE_ORDRE;
       }
       break;
 
     case MATCH_EN_COURS:
-      if(Strategie(couleur) == ACTION_TERMINEE){
-        index_Maitre = ATTENTE_ORDRE;
+      Serial.printf(">tirette_m:%d\n", tirette_enlevee);
+      if(tirette_enlevee == 0){
+        lire_tirette(&tirette_enlevee);
+      }
+      if(tirette_enlevee == 1){
+        if(Strategie(couleur) == ACTION_TERMINEE){
+          index_Maitre = ATTENTE_ORDRE;
+        }
       }
       break;
 
@@ -428,73 +442,114 @@ void IHM_attente_match(int * couleur){
 
 enum etat_action_t Strategie(int couleur){
   static enum {
+    STRAT_INIT, // On attend de ne pas voir la tirette dans la detection de l'adversaire
     STRAT_RECULE_BANDEROLE, // Deplacement relatif
-    STRAT_ALLER_GRADINS_1_A,  // Déplacement absolu
-    STRAT_ALLER_GRADINS_1_B,  // Déplacement relatif
+    STRAT_ALLER_GRADINS_1_A,  // Déplacement relatif
+    STRAT_ALLER_GRADINS_1_B,  // Cherche gradin
     STRAT_ALLER_GRADINS_1_C,  // Déplacement relatif
-    STRAT_ALLER_PREPA_BACKSTAGE, // Déplacement absolu
-    STRAT_ALLER_BACKSTAGE // Déplacement relatif
+    STRAT_DEPOSE_GRADIN_1A, // Empile gradin
+    STRAT_DEPOSE_GRADIN_1B, // Recule du gradin
+    STRAT_ALLER_PREPA_BACKSTAGE, // Déplacement relaif
+    STRAT_ATTENTE_BACKSTAGE, // Attente
+    STRAT_ALLER_BACKSTAGE, // Déplacement relatif
+    STRAT_FIN
 
-  }etat_strategie = STRAT_RECULE_BANDEROLE;
+  }etat_strategie = STRAT_INIT;
   enum etat_action_t etat_action;
   int translation_x_mm, translation_y_mm;
+  static unsigned long temps_timer;
   float rotation_rad;
 
   switch(etat_strategie){
+    case STRAT_INIT:
+      temps_timer = millis();
+      etat_strategie = STRAT_RECULE_BANDEROLE;
+      break;
     case STRAT_RECULE_BANDEROLE:
-      // Déplacement en X
-      translation_x_mm = -450;
-      translation_y_mm = 0;
-      rotation_rad = 0;
-      etat_action = deplacement_relatif(translation_x_mm, translation_y_mm, rotation_rad, 1);
-      if(etat_action == ACTION_TERMINEE){
-        etat_strategie = STRAT_ALLER_GRADINS_1_A;
+      if(millis() - temps_timer >300){
+        // Déplacement en X
+        translation_x_mm = -300;
+        translation_y_mm = 0;
+        rotation_rad = 0;
+        etat_action = deplacement_relatif(translation_x_mm, translation_y_mm, rotation_rad, 1);
+        if(etat_action == ACTION_TERMINEE){
+          etat_strategie = STRAT_ALLER_GRADINS_1_A;
+        }
       }
       break;
 
     case STRAT_ALLER_GRADINS_1_A:
+      translation_x_mm = -450;
+      rotation_rad = 0;
       if(couleur == COULEUR_JAUNE){
-        etat_action = deplacement_absolu(800, 800, -M_PI/2., 1);
+        translation_y_mm = -450;
       }else{
-        etat_action = deplacement_absolu(3000 - 800, 800, -M_PI/2., 1);
+        translation_y_mm = 450;
       }
+      etat_action = deplacement_relatif(translation_x_mm, translation_y_mm, rotation_rad, 1);
       if(etat_action == ACTION_TERMINEE){
         etat_strategie = STRAT_ALLER_GRADINS_1_B;
       }
       break;
 
     case STRAT_ALLER_GRADINS_1_B:
-      etat_action = deplacement_relatif(300, 0, 0, 0);
-      if(etat_action == ACTION_TERMINEE){
+      etat_action = gradin_approche();
+      if(etat_action == ACTION_TERMINEE || etat_action == ACTION_ECHEC){
         etat_strategie = STRAT_ALLER_GRADINS_1_C;
       }
       break;
 
     case STRAT_ALLER_GRADINS_1_C:
-      etat_action = deplacement_relatif(-250, 0, 0, 0);
+      etat_action = deplacement_relatif(100, 0, 0, 0);
       if(etat_action == ACTION_TERMINEE){
-        etat_strategie = STRAT_ALLER_PREPA_BACKSTAGE;
+        etat_strategie = STRAT_DEPOSE_GRADIN_1A;
       }
       break;
 
+    case STRAT_DEPOSE_GRADIN_1A:
+      etat_action = actionneur_action(ACTIONNEUR_EMPILE);
+      if(etat_action == ACTION_TERMINEE){
+        etat_strategie = STRAT_DEPOSE_GRADIN_1B;
+      }
+      break;
+
+    case STRAT_DEPOSE_GRADIN_1B:
+      translation_x_mm = -250;
+      rotation_rad = 0;
+      translation_y_mm = 0;
+      etat_action = deplacement_relatif(translation_x_mm, translation_y_mm, rotation_rad, 1);
+      if(etat_action == ACTION_TERMINEE){
+        etat_strategie = STRAT_ALLER_PREPA_BACKSTAGE;
+      }
+
+
     case STRAT_ALLER_PREPA_BACKSTAGE:
       if(couleur == COULEUR_JAUNE){
-        etat_action = deplacement_absolu(550, 1150, M_PI/2., 0);
+        etat_action = deplacement_relatif(0, 0, -15. / 180 * M_PI, 0);
       }else{
-        etat_action = deplacement_absolu(3000 - 550, 1150, M_PI/2., 0);
+        etat_action = deplacement_relatif(0, 0, 15. / 180 * M_PI, 0);
       }
       if(etat_action == ACTION_TERMINEE){
+        etat_strategie = STRAT_ATTENTE_BACKSTAGE;
+      }
+      break;
+
+    case STRAT_ATTENTE_BACKSTAGE:
+      if (millis() - temps_timer > 95000){
         etat_strategie = STRAT_ALLER_BACKSTAGE;
       }
       break;
 
     case STRAT_ALLER_BACKSTAGE:
-      etat_action = deplacement_relatif(500, 0, 0, 0);
+      etat_action = deplacement_relatif(-1140, 0, 0, 1);
       if(etat_action == ACTION_TERMINEE){
-        etat_strategie = STRAT_RECULE_BANDEROLE;
-        return ACTION_TERMINEE;
+        etat_strategie = STRAT_FIN;
       }
       break;
+
+    case STRAT_FIN:
+      return ACTION_TERMINEE;
+      break;
   }
 
   return ACTION_EN_COURS;
@@ -510,7 +565,8 @@ enum etat_action_t gradin_approche(void){
     GA_GOTO_EN_FACE,
     GA_TOURNE_LARGE,
     GA_GOTO_PROCHE,
-    GA_GOTO_PREND,
+    GA_GOTO_PREND_1,
+    GA_GOTO_PREND_2,
     GA_RECULE
   } statu_approche_gradin = GA_INIT;
   static float angle_parcouru, angle_mem;
@@ -528,6 +584,7 @@ enum etat_action_t gradin_approche(void){
 
   switch(statu_approche_gradin){
     case GA_INIT:
+      while(actionneur_action(ACTIONNEUR_DEPLACEMENT)!=ACTION_TERMINEE);
       angle_parcouru = 0;
       statu_approche_gradin = GA_CHERCHE_GAUCHE;
       break;
@@ -576,7 +633,6 @@ enum etat_action_t gradin_approche(void){
 
     case GA_GOTO_EN_FACE:
     case GA_GOTO_LARGE:
-      Detect_gradin(&detect_gradin);
       if(detect_gradin.status != 2){
         nb_erreur++;
         if(nb_erreur > 100){
@@ -590,10 +646,12 @@ enum etat_action_t gradin_approche(void){
         // On se centre par rapport au gradin
         //translation_x = (detect_gradin.centre_y_mm + 150) * tan(fabs(detect_gradin.angle_rad));
         //translation_y = -(detect_gradin.centre_y_mm + 150) * sin(detect_gradin.angle_rad) - detect_gradin.centre_x_mm;
-        translation_x = (detect_gradin.centre_y_mm + 150) * tan(fabs(detect_gradin.angle_rad));
-        translation_y = -(detect_gradin.centre_y_mm + 150) * sin(detect_gradin.angle_rad) - detect_gradin.centre_x_mm;
+        /// Attention, réflection dans le repère des capteurs
+        translation_x = (detect_gradin.centre_y_mm + 75) * sin(detect_gradin.angle_rad) * cos(detect_gradin.angle_rad) + detect_gradin.centre_x_mm;
+        translation_y = (detect_gradin.centre_y_mm + 75) * sin(detect_gradin.angle_rad) * sin(detect_gradin.angle_rad) ;
 
-        while(deplacement_relatif(translation_x, translation_y, 0, 0) == ACTION_TERMINEE);
+        // Changement de repère : capteurs => Chassis
+        while(deplacement_relatif(translation_y, -translation_x, 0, 0) != ACTION_TERMINEE);
         statu_approche_gradin = GA_TOURNE_LARGE;
         angle_mem = detect_gradin.angle_rad;
         
@@ -602,13 +660,18 @@ enum etat_action_t gradin_approche(void){
 
     case GA_TOURNE_LARGE:
       while(deplacement_relatif(0, 0, -angle_mem, 0) != ACTION_TERMINEE);
+      /*//// !!! DEBUG
+      statu_approche_gradin = GA_INIT;
+      return ACTION_ECHEC;
+      /// FIN DEBUG*/
       delay(150);
       Detect_gradin(&detect_gradin);
       if(detect_gradin.status != 2){
         statu_approche_gradin = GA_CHERCHE_GAUCHE;
       }else if(detect_gradin.centre_y_mm < 180){
         statu_approche_gradin = GA_RECULE;
-      }else if(fabs(detect_gradin.angle_rad) > GRADIN_PRECISION_ANGLE_RAD){
+      }else if((fabs(detect_gradin.angle_rad) > GRADIN_PRECISION_ANGLE_RAD * 6) && (detect_gradin.centre_y_mm >= 240) ||
+              (fabs(detect_gradin.angle_rad) > GRADIN_PRECISION_ANGLE_RAD) && (detect_gradin.centre_y_mm < 240)){
         angle_mem = detect_gradin.angle_rad;
       }else{
         if(detect_gradin.centre_y_mm > 240){
@@ -617,7 +680,7 @@ enum etat_action_t gradin_approche(void){
           if(fabs(detect_gradin.centre_x_mm) > 10){
             statu_approche_gradin = GA_GOTO_LARGE;
           }else{
-            statu_approche_gradin = GA_GOTO_PREND;
+            statu_approche_gradin = GA_GOTO_PREND_1;
           }
         }
       }
@@ -630,14 +693,37 @@ enum etat_action_t gradin_approche(void){
       }
       break;
 
-    case GA_GOTO_PREND:
-      while(deplacement_relatif((detect_gradin.centre_y_mm - 25), 0, 0, 0) != ACTION_TERMINEE);
-      statu_approche_gradin = GA_INIT;
-      delay(5000);
-      return ACTION_TERMINEE;
-  
+    case GA_GOTO_PREND_1:
+      while(deplacement_relatif((detect_gradin.centre_y_mm - 25 - 110), 0, 0, 0) != ACTION_TERMINEE);
+      statu_approche_gradin = GA_GOTO_PREND_2;
       break;
 
+    case GA_GOTO_PREND_2:
+      // On positionne la fourche
+      while(actionneur_action(ACTIONNEUR_PREPARE_PRISE_PLANCHE)!=ACTION_TERMINEE);
+      // On Avance de 15 cm
+      while(deplacement_relatif(150, 0, 0, 0) != ACTION_TERMINEE);
+      // Depile les planches
+      while(actionneur_action(ACTIONNEUR_DEPILE)!=ACTION_TERMINEE);
+      // Recule de 4 cm
+      while(deplacement_relatif(-40, 0, 0, 0) != ACTION_TERMINEE);
+      // Prepa prise externe
+      while(actionneur_action(ACTIONNEUR_PRISE_EXTERNE)!=ACTION_TERMINEE);
+      // Avance de 4,4 cm
+      while(deplacement_relatif(65, 0, 0, 0) != ACTION_TERMINEE);
+      // On attrape les canettes internes
+      while(actionneur_action(ACTIONNEUR_PRISE)!=ACTION_TERMINEE);
+      // On recule pour ne pas faire tomber la fourche sur les cannettes
+      //while(deplacement_relatif(-20, 0, 0, 0) != ACTION_TERMINEE);
+      // On baisse la fourche
+      //while(actionneur_action(ACTIONNEUR_FOURCHE_TRANSPORT)!=ACTION_TERMINEE);
+
+      statu_approche_gradin = GA_CHERCHE_GAUCHE;
+      return ACTION_TERMINEE;
+      break;
+      
+
+
     case GA_RECULE:
       while(deplacement_relatif((detect_gradin.centre_y_mm - 220), 0, 0, 0) != ACTION_TERMINEE);
       statu_approche_gradin = GA_CHERCHE_GAUCHE;
@@ -791,8 +877,47 @@ int detection_adversaire(float angle_deplacement){
   return 0;
 }
 
+#define SEUIL_OBSTACLE_CM 50
+
+/// @brief A partir des déplacements X et Y, selection des capteurs à utiliser pour détecter l'adversaire
+/// @return 1 obstable, 0 pas d'obstacle
+int presence_obstacle(int distance_x_mm, int distance_y_mm){
+  struct detect_adv_reception_t detect_adv_reception;
+  Detect_adv_lire(&detect_adv_reception);
+  float angle = atan2f(distance_y_mm, distance_x_mm);
+  if(angle < 0){
+    angle += 2 * M_PI;
+  }
+  // Angle entre 0 et 2 PI
+  int segment_central = 23 - angle / 2. / M_PI * 24;
+  // segment_central entre 0 et 23;
+  if(segment_central == 23){
+    segment_central = 0;
+  }
+  // segment_central entre 0 et 22;
+  int capteur_central =  segment_central / 2;
+  int capteur_suivant = capteur_central + 1;
+  int capteur_precedent = capteur_central - 1;
+
+  if(capteur_suivant > 11){
+    capteur_suivant = 0;
+  }
+  if(capteur_precedent < 0){
+    capteur_precedent = 11;
+  }
+
+  if(detect_adv_reception.distance_cm[capteur_precedent] < SEUIL_OBSTACLE_CM || 
+      detect_adv_reception.distance_cm[capteur_central] < SEUIL_OBSTACLE_CM || 
+      detect_adv_reception.distance_cm[capteur_suivant] < SEUIL_OBSTACLE_CM ){
+    // Arrêt du mouvement
+    return 1;
+  }
+  return 0;
+
+}
+
 /// @brief Deplacement dans le repère du robot, pouvant prendre en compte la detection de l'adversaire
-/// evitement : 1 pour s'arreter si adversaire detecté, 0 pour ignorer l'adversaire
+/// @param evitement : 1 pour s'arreter si adversaire detecté, 0 pour ignorer l'adversaire
 enum etat_action_t deplacement_relatif(int distance_x_mm, int distance_y_mm, float rotation_rad, int evitement){
   static enum{
     DR_INIT,
@@ -801,7 +926,6 @@ enum etat_action_t deplacement_relatif(int distance_x_mm, int distance_y_mm, flo
 
   struct chassis_emission_t chassis_emission;
   struct chassis_reception_t chassis_reception;
-  struct detect_adv_reception_t detect_adv_reception;
 
   switch(etat_deplacement){
     case DR_INIT:
@@ -819,21 +943,11 @@ enum etat_action_t deplacement_relatif(int distance_x_mm, int distance_y_mm, flo
 
     case DR_MVT_EN_COUR:
       if(evitement){
-        // On lit les capteurs
-        Detect_adv_lire(&detect_adv_reception);
-        // On analyse les valeurs - TODO : créer une fonction plus évoluée
-        if(distance_x_mm > 0){
-          if(detect_adv_reception.distance_cm[0] < 50){
-            // Arrêt du mouvement
-            chassis_emission.status = MOUVEMENT_INTERRUPTION;
-            send_Chassis(&chassis_emission);
-          }
-        }else if(distance_x_mm < 0){
-          if(detect_adv_reception.distance_cm[6] < 50){
-            // Arrêt du mouvement
-            chassis_emission.status = MOUVEMENT_INTERRUPTION;
-            send_Chassis(&chassis_emission);
-          }
+        if(presence_obstacle(distance_x_mm, distance_y_mm)){
+          chassis_emission.status = MOUVEMENT_INTERRUPTION;
+          send_Chassis(&chassis_emission);
+          etat_deplacement = DR_INIT;
+          return ACTION_ECHEC;
         }
       }
 
@@ -850,6 +964,38 @@ enum etat_action_t deplacement_relatif(int distance_x_mm, int distance_y_mm, flo
   return ACTION_EN_COURS;
 }
 
+enum etat_action_t actionneur_pos_initiale(){
+
+}
+
+enum etat_action_t actionneur_action(int action){
+  struct com_actionneur_t com_actionneur_env, com_actionneur_rec;
+  int action_terminee=0;
+
+  com_actionneur_env.demande_action = action;
+  com_actionneur_rec.action_terminee = 0;
+  Actionneur_send(&com_actionneur_env);
+
+  do{
+    Actionneur_scan(&com_actionneur_rec);
+  }while(com_actionneur_rec.action_terminee != com_actionneur_env.demande_action);
+
+  // On réinitialise la machine à état
+  com_actionneur_env.demande_action = 0;
+  Actionneur_send(&com_actionneur_env);
+  Serial.printf("Lecture action...\n");
+  do{
+    Actionneur_scan(&com_actionneur_rec);
+  }while(com_actionneur_rec.action_terminee != com_actionneur_env.demande_action);
+
+  return ACTION_TERMINEE;
+
+
+
+
+}
+
+
 
 void scan_I2C_bus(){
   char error, address;

Cerveau/Com_actionneur.h

@@ -0,0 +1,20 @@
+#ifndef COM_ACTIONNEUR_H
+#define COM_ACTIONNEUR_H
+
+#define I2C_SLAVE_actionneur 0x20
+
+#define ACTIONNEUR_NO_ACTION 0
+#define ACTIONNEUR_POS_INITIAL 1
+#define ACTIONNEUR_DEPLACEMENT 2
+#define ACTIONNEUR_PREPARE_PRISE_PLANCHE 3
+#define ACTIONNEUR_PRISE 4
+#define ACTIONNEUR_EMPILE 5
+#define ACTIONNEUR_DEPILE 6
+#define ACTIONNEUR_PRISE_EXTERNE 7
+#define ACTIONNEUR_FOURCHE_TRANSPORT 8
+
+struct com_actionneur_t{
+  char demande_action, action_terminee;
+};
+
+#endif //COM_ACTIONNEUR_H

Cerveau/Com_actionneur.ino

@@ -0,0 +1,40 @@
+#include "Com_actionneur.h"
+#include <Arduino.h>
+#include <HardwareSerial.h>
+
+
+
+int Actionneur_scan(struct com_actionneur_t * com_actionneur){
+  return Actionneur_scan(com_actionneur, true);
+}
+/// @brief Commande l'actionneur
+int Actionneur_scan(struct com_actionneur_t * com_actionneur, bool continuous_try){
+  unsigned char tampon[4];
+  //(Adresse I2c, Adresse dans le registre, tampon, longueur de donnée)
+  error = I2C_lire_registre(I2C_SLAVE_actionneur, 0, tampon, 2);
+  while(error !=0 && continuous_try){
+    affiche_erreur("Actionneur_scan", "Erreur I2C");
+    error = I2C_lire_registre(I2C_SLAVE_actionneur, 0, tampon, 2);
+  }
+
+  Serial.printf(">r0:%d\n>r1:%d\n", tampon[0], tampon[1]);
+  com_actionneur->action_terminee = tampon[1];
+  com_actionneur->demande_action = tampon[0];
+  
+  return error;
+}
+
+int Actionneur_send(struct com_actionneur_t * com_actionneur){
+  
+  unsigned char tampon[1];
+  
+
+  tampon[0] = com_actionneur->demande_action;
+  
+  error = I2C_ecrire_registre(I2C_SLAVE_actionneur, 0, tampon, 1);
+  if (error !=0){
+    affiche_erreur("Actionneur_send", "Erreur I2C");
+    while(1);
+  }
+  return error;
+}

Cerveau/Com_chassis.ino

@@ -8,23 +8,23 @@ int Scan_chassis(struct chassis_reception_t * chassis_reception){
 }
 
 /// @brief Lit l'état du chassis en I2C
-int Scan_chassis(struct chassis_reception_t * chassis_reception, bool blocking){
+int Scan_chassis(struct chassis_reception_t * chassis_reception, bool continuous_try){
   unsigned char tampon2[14];
   //(Adresse I2c, Adresse dans le registre, tampon, longueur de donnée)
   error = I2C_lire_registre(I2C_SLAVE_chassi, 0, tampon2, 12);
-  if (error !=0){
+  while (error !=0 && continuous_try){
     Err_Chassi_com =1;IndexErr = 2;
     affiche_erreur("Scan_Chassi", "Erreur I2C");
-    if(blocking){
-      while(1);
-    }
-  }else{
+    error = I2C_lire_registre(I2C_SLAVE_chassi, 0, tampon2, 12);  
+  }
+  if(error == 0){
     Err_Chassi_com =0;
     IndexErr = 0;
 
     Mvt_finit = (MOUVEMENT_FINI == tampon2[0]);
     chassis_reception->status = tampon2[0];
   }
+
   return error;
 }
 
@@ -35,9 +35,9 @@ void send_Chassis(struct chassis_emission_t * chassis_emission){
 
     // Conversion des mm ou radian en pas
 
-    nb_pas_x = chassis_emission->translation_x_mm * 4.049;
+    nb_pas_x = chassis_emission->translation_x_mm * -4.049;
     nb_pas_y = chassis_emission->translation_y_mm * 4.953;
-    nb_pas_rot = chassis_emission->rotation_z_rad * 791.;
+    nb_pas_rot = chassis_emission->rotation_z_rad * -791.;
 
     Mot[0] = chassis_emission->status;
     //y*=-1;
@@ -55,8 +55,11 @@ void send_Chassis(struct chassis_emission_t * chassis_emission){
     Mot[10] = chassis_emission->acceleration; 
 
     error = I2C_ecrire_registre(I2C_SLAVE_chassi, 0, Mot, 11);
-    if (error !=0){Err_Cha_send =1; IndexErr = 5;}
-    else{Err_Cha_send =0;IndexErr = 0;}
+    while (error !=0){
+      error = I2C_ecrire_registre(I2C_SLAVE_chassi, 0, Mot, 11);
+      Err_Cha_send =1; IndexErr = 5;
+    }
+    Err_Cha_send =0;IndexErr = 0;
     if(error==0){   //si pas d'erreur de transmission alors RAZ des valeurs
       nbr_essai ++;
       cmd_chassi_x = 0;

Cerveau/Com_detection_adversaire.ino

@@ -6,17 +6,14 @@ int Detect_adv_lire(struct detect_adv_reception_t * detect_adv_reception){
   return Detect_adv_lire(detect_adv_reception, true);
 }
 /// @brief Lit les capteurs VL53L1X
-int Detect_adv_lire(struct detect_adv_reception_t * detect_adv_reception, bool blocking){
+int Detect_adv_lire(struct detect_adv_reception_t * detect_adv_reception, bool continuous_try){
   unsigned char tampon2[14];
   //(Adresse I2c, Adresse dans le registre, tampon, longueur de donnée)
   error = I2C_lire_registre(I2C_SLAVE_detect_adv, 0, detect_adv_reception->distance_cm, 12);
-  if (error !=0){
+  while (error !=0 && continuous_try){
     affiche_erreur("Scan_Detect_adversaire", "Erreur I2C");
-    if(blocking){
-      while(1);
-    }
-  }else{
-    Serial.println("I2C OK");
+    error = I2C_lire_registre(I2C_SLAVE_detect_adv, 0, detect_adv_reception->distance_cm, 12);
+    
   }
   return error;
 }

Cerveau/Com_gradins.h

@@ -9,4 +9,6 @@ struct detect_gradin_t{
   float angle_rad;
 };
 
+int lire_tirette(void);
+
 #endif

Cerveau/Com_gradins.ino

@@ -30,3 +30,22 @@ int Detect_gradin(struct detect_gradin_t * detect_gradin, bool blocking){
   }
   return error;
 }
+
+int lire_tirette(char * tirette){
+  return lire_tirette(tirette, true);
+}
+
+
+int lire_tirette(char * tirette, bool continuous_try){
+  unsigned char tampon;
+  //(Adresse I2c, Adresse dans le registre, tampon, longueur de donnée)
+  error = I2C_lire_registre(I2C_SLAVE_detect_gradin, 0x0d, &tampon, 1);
+  while(error !=0 && continuous_try){
+    affiche_erreur("lire_tirette", "Erreur I2C");
+    error = I2C_lire_registre(I2C_SLAVE_detect_gradin, 0x0d, &tampon, 1);
+  }
+  *tirette = tampon;
+  Serial.printf(">tirette:%d", *tirette);
+  return error;
+
+}

Cerveau/I2C_Master_lib.ino

@@ -26,7 +26,7 @@ uint8_t I2C_lire_registre(int adresse_i2c, uint adresse_registre, unsigned char
 
   WIRE.requestFrom(adresse_i2c, taille_donnees + 1);
   // ceci contient l'adresse
-  if(adresse_i2c == I2C_SLAVE_chassi){
+  if(adresse_i2c == I2C_SLAVE_chassi || adresse_i2c  == I2C_SLAVE_actionneur){
     Wire.readBytes(&temp, 1);
   }
   // Et ceci les données

Cerveau/ServerWeb.ino

@@ -1,6 +1,7 @@
 #include <Arduino.h>
 #include <WebServer.h>
 
+
 //#include "Chassis.h"
 //#include "ServerWeb.h"
 
@@ -101,23 +102,26 @@ void handleForm() {
 }
 
 void showStatus() {
-  char chassis[50], gradin[50], triangulation[50], detection_adversaire[50];
+  char chassis[50], gradin[50], triangulation[50], detection_adversaire[50], actionneur[50];
   char message_statu[500];
   const char message_statu_tmplt[] = "<!DOCTYPE html><html><head><meta charset=\"UTF-8\"><title>A simple form</title></head>\
   <body>\n<h1>Statu du robot</h1>\n\
-  <p>Chassis: %s</p><p>Gradin: %s</p><p>Triangulation: %s</p><p>Detection_adversaire: %s</p>";
+  <p>Chassis: %s</p><p>Gradin: %s</p><p>Triangulation: %s</p><p>Detection_adversaire: %s</p><p>Actionneur (0x20): %s</p>";
   struct chassis_reception_t chassis_reception;
   struct detect_gradin_t detect_gradin;
   struct triangulation_reception_t triangulation_reception;
   struct detect_adv_reception_t detect_adv_reception;
+  struct com_actionneur_t com_actionneur;
 
   statu_to_string(chassis, !Scan_chassis(&chassis_reception, false) );
   statu_to_string(gradin, !Detect_gradin(&detect_gradin, false) );
   statu_to_string(triangulation, !Scan_Triangulation(&triangulation_reception, false) );
   statu_to_string(detection_adversaire, !Detect_adv_lire(&detect_adv_reception, false) );
+  statu_to_string(actionneur, !Actionneur_scan(&com_actionneur, false) );
 
 
-  sprintf(message_statu, message_statu_tmplt, chassis, gradin, triangulation, detection_adversaire);
+
+  sprintf(message_statu, message_statu_tmplt, chassis, gradin, triangulation, detection_adversaire, actionneur);
   server.send(200, "text/html", message_statu);
   
 }

Chassis/Chassis.ino

@@ -232,7 +232,7 @@ void Mvmt(){
       // Serial.print("X Y Z A !");Serial.print("\t\t");Serial.print("\tinter");Serial.println("\tModif");
       // Serial.print(stepperX.distanceToGo());Serial.print("/");Serial.print(stepperY.distanceToGo());Serial.print("/");Serial.print(stepperZ.distanceToGo());Serial.print("/");Serial.print(stepperA.distanceToGo());Serial.print("\t");Serial.print(Interrupt);Serial.print("\t");Serial.println(Modif_Mvt);
     #endif
-    while((stepperX.distanceToGo() != 0 || stepperY.distanceToGo() != 0 || stepperZ.distanceToGo() != 0 || stepperA.distanceToGo() != 0) /*&& (Interrupt != 1 || Modif_Mvt != 1)*/){
+    while((stepperX.distanceToGo() != 0 || stepperY.distanceToGo() != 0 || stepperZ.distanceToGo() != 0 || stepperA.distanceToGo() != 0) && (I2C_memory[0] != 1)){
         stepperX.run(); // X
         stepperY.run(); // Y
         stepperZ.run(); // Z
@@ -244,6 +244,23 @@ void Mvmt(){
         // delay(100);
         #endif
     }
+    // Bien bloquer le robot en cas d'interruption
+    if(I2C_memory[0] == 1){
+      stepperX.setAcceleration(40000);
+      stepperY.setAcceleration(40000);
+      stepperZ.setAcceleration(40000);
+      stepperA.setAcceleration(40000);
+      stepperX.stop(); // X
+      stepperY.stop(); // Y
+      stepperZ.stop(); // Z
+      stepperA.stop(); // A
+      while(stepperX.distanceToGo() != 0 || stepperY.distanceToGo() != 0 || stepperZ.distanceToGo() != 0 || stepperA.distanceToGo() != 0){
+        stepperX.run(); // X
+        stepperY.run(); // Y
+        stepperZ.run(); // Z
+        stepperA.run(); // A
+      }
+    }
   }  
   // if(Modif_Mvt == 1){
   //   Position_Calculation();