Amélioration de l'approche des gradins - correction d'un vilain bug

Cerveau/Cerveau.ino

@@ -365,7 +365,7 @@ 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;
@@ -624,10 +624,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;
         
@@ -636,6 +638,10 @@ 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){
@@ -835,8 +841,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,
@@ -845,7 +890,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:
@@ -863,21 +907,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;
         }
       }
 

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();