Test évitemment ok sur fonction de test, soucis lors de l'Homologation

.vscode/settings.json

@@ -1,5 +1,8 @@
 {
     "files.associations": {
-        "timer.h": "c"
+        "timer.h": "c",
+        "localisation.h": "c",
+        "math.h": "c",
+        "strategie.h": "c"
     }
 }

Balise_VL53L1X.c

@@ -1,10 +1,11 @@
+#include <stdio.h>
 #include "i2c_annexe.h"
 #include "Localisation.h"
 #include "math.h"
 
 #define NB_CAPTEURS 12
 #define DISTANCE_OBSTACLE_IGNORE 200
-#define DISTANCE_CAPTEUR_CENTRE_ROBOT 40
+#define DISTANCE_CAPTEUR_CENTRE_ROBOT_CM 4
 
 uint8_t distance_capteur_cm[NB_CAPTEURS];
 
@@ -12,9 +13,11 @@ struct capteur_VL53L1X_t{
     uint8_t distance_cm;
     double angle_ref_robot;
     double angle_ref_terrain;
+    uint donnee_valide;
 }capteurs_VL53L1X[NB_CAPTEURS];
 
 void actualise_VL53L1X(struct capteur_VL53L1X_t * capteur_VL53L1X, uint8_t distance_capteur_cm);
+void invalide_obstacle(struct capteur_VL53L1X_t *capteur_VL53L1X, struct position_t position_robot);
 
 void Balise_VL53L1X_gestion(){
     i2c_annexe_get_distances(distance_capteur_cm);
@@ -24,8 +27,10 @@ void Balise_VL53L1X_gestion(){
 }
 
 void actualise_VL53L1X(struct capteur_VL53L1X_t * capteur_VL53L1X, uint8_t distance_capteur_cm){
+    struct position_t position_robot;
+    position_robot = Localisation_get();
     // Actualisation de l'angle du capteur
-    capteur_VL53L1X->angle_ref_terrain = capteur_VL53L1X->angle_ref_robot + Localisation_get().angle_radian;
+    capteur_VL53L1X->angle_ref_terrain = capteur_VL53L1X->angle_ref_robot + position_robot.angle_radian;
     // Maintien de l'angle entre -PI et PI
     while(capteur_VL53L1X->angle_ref_terrain > 2* M_PI){
         capteur_VL53L1X->angle_ref_terrain -= 2* M_PI;
@@ -33,7 +38,33 @@ void actualise_VL53L1X(struct capteur_VL53L1X_t * capteur_VL53L1X, uint8_t dista
     while(capteur_VL53L1X->angle_ref_terrain < 2* -M_PI){
         capteur_VL53L1X->angle_ref_terrain += 2* M_PI;
     }
-    capteur_VL53L1X->distance_cm = distance_capteur_cm + DISTANCE_CAPTEUR_CENTRE_ROBOT;
+    capteur_VL53L1X->distance_cm = distance_capteur_cm + DISTANCE_CAPTEUR_CENTRE_ROBOT_CM;
+
+    invalide_obstacle(capteur_VL53L1X, position_robot);
+
+}
+
+/// @brief Definit si l'obstable doit être pris en comptre
+/// @param  
+void invalide_obstacle(struct capteur_VL53L1X_t *capteur_VL53L1X, struct position_t position_robot){
+    // Positionne l'obstacle sur le terrain
+    struct position_t position_obstacle;
+    //printf("Angle:%.1f\n",capteur_VL53L1X->angle_ref_terrain);
+    position_obstacle.x_mm = position_robot.x_mm + cos(capteur_VL53L1X->angle_ref_terrain)* (double)(capteur_VL53L1X->distance_cm * 10);
+    position_obstacle.y_mm = position_robot.y_mm + sin(capteur_VL53L1X->angle_ref_terrain)* (double)(capteur_VL53L1X->distance_cm * 10);
+
+    capteur_VL53L1X->donnee_valide=1;
+    // Si la distance vaut 0, à invalider
+    if(capteur_VL53L1X->distance_cm <= DISTANCE_CAPTEUR_CENTRE_ROBOT_CM){
+        capteur_VL53L1X->donnee_valide=0;
+    }
+    // Si l'obstacle est à l'extérieur du terrain (on prend 50 mm de marge à l'intérieur du terrain, la balise faisant 100 mm)
+    /*printf("X:%.1f,Y:%.1f\n", position_obstacle.x_mm, position_obstacle.y_mm);
+    if((position_obstacle.x_mm < 50) || (position_obstacle.y_mm < 50) || (position_obstacle.x_mm > 1950) || (position_obstacle.y_mm > 2950))
+    {
+        capteur_VL53L1X->donnee_valide=0;
+    }*/
+
 }
 
 void Balise_VL53L1X_init(){
@@ -54,10 +85,12 @@ void Balise_VL53L1X_init(){
 uint8_t Balise_VL53L1X_get_min_distance(void){
     uint8_t min_distance = DISTANCE_OBSTACLE_IGNORE;
     for(uint8_t capteur=0; capteur<NB_CAPTEURS; capteur++){
+        if(capteurs_VL53L1X[capteur].donnee_valide){
             if(min_distance> capteurs_VL53L1X[capteur].distance_cm){
                 min_distance = capteurs_VL53L1X[capteur].distance_cm;
             }
         }
+    }
     return min_distance;
 }
 

Strategie.c

@@ -1,6 +1,7 @@
 #include "hardware/gpio.h"
 #include "i2c_annexe.h"
 #include "Asser_Position.h"
+#include "Balise_VL53L1X.h"
 #include "Geometrie_robot.h"
 #include "Localisation.h"
 #include "Moteurs.h"
@@ -9,11 +10,10 @@
 #include "Trajet.h"
 #include "math.h"
 
-#define DEGREE_EN_RADIAN  (M_PI / 180.)
 #define SEUIL_RECAL_DIST_MM 75
 #define SEUIL_RECAL_ANGLE_RADIAN (5 * DEGREE_EN_RADIAN)
+#define DISTANCE_OBSTACLE_CM 35
 
-enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint32_t step_ms);
 enum etat_action_t calage_angle(enum longer_direction_t longer_direction, double x_mm, double y_mm, double angle_radian);
 enum etat_action_t lance_balles(uint32_t step_ms);
 
@@ -195,6 +195,10 @@ enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint
             break;
         
         case PARCOURS_AVANCE:
+            if(Balise_VL53L1X_get_min_distance() <DISTANCE_OBSTACLE_CM){
+                Trajet_avance(0.);
+                break;
+            }
             etat_trajet = Trajet_avance(step_ms/1000.);
             if(etat_trajet == TRAJET_TERMINE){
                 etat_action = ACTION_TERMINEE;

Strategie.h

@@ -1,10 +1,12 @@
 #include "pico/stdlib.h"
+#include "Trajectoire.h"
 
 #ifndef STRATEGIE_H
 #define STRATEGIE_H
 
 #define COULEUR 15
 #define TIRETTE 14
+#define DEGREE_EN_RADIAN  (M_PI / 180.)
 
 enum etat_action_t{
     ACTION_EN_COURS,
@@ -36,6 +38,7 @@ extern enum etat_strategie_t{
 
 enum etat_action_t cerise_accostage(void);
 enum etat_action_t avance_puis_longe_bordure(enum longer_direction_t longer_direction);
+enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint32_t step_ms);
 void Homologation(uint32_t step_ms);
 enum couleur_t lire_couleur(void);
 uint attente_tirette(void);

Test.c

@@ -223,6 +223,8 @@ int test_continue_test(){
 int test_capteurs_balise(void){
     printf("Test de la balise\n");
     i2c_maitre_init();
+    Localisation_set(0,0,0);
+    Balise_VL53L1X_init();
     
     while(true){
         uint8_t min_distance;
@@ -235,6 +237,7 @@ int test_capteurs_balise(void){
         for(uint8_t capteur=0; capteur<12; capteur++){
             printf(">c%d:%d\n",capteur, Balise_VL53L1X_get_capteur_cm(capteur));
         }
+        sleep_ms(20);
     }
 
 }

Test_strategie.c

@@ -4,6 +4,7 @@
 #include "math.h"
 
 #include "Asser_Moteurs.h"
+#include "Balise_VL53L1X.h"
 #include "i2c_annexe.h"
 #include "i2c_maitre.h"
 #include "gyro.h"
@@ -19,6 +20,7 @@
 int test_accostage(void);
 int test_longe(void);
 int test_homologation(void);
+int test_evitement(void);
 int test_tirette_et_couleur();
 
 void affichage_test_strategie(){
@@ -78,6 +80,11 @@ int test_strategie(){
             while(test_tirette_et_couleur());
             break;
 
+        case 'e':
+        case 'E':
+            while(test_evitement());
+            break;
+
         case 'h':
         case 'H':
             while(test_homologation());
@@ -149,6 +156,78 @@ int test_homologation(){
 
 }
 
+void affichage_test_evitement(){
+    while(1){
+        printf(">min_dist:%d\n",Balise_VL53L1X_get_min_distance());
+        for(uint8_t capteur=0; capteur<12; capteur++){
+            printf(">c%d:%d\n",capteur, Balise_VL53L1X_get_capteur_cm(capteur));
+        }
+    }
+}
+
+int test_evitement(){
+    int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
+    struct trajectoire_t trajectoire;
+    printf("Evitement\n");
+
+    i2c_maitre_init();
+    Trajet_init();
+    Balise_VL53L1X_init();
+    Localisation_set(200,200,0);
+    //printf("Init gyroscope\n");
+    set_position_avec_gyroscope(0);
+    if(get_position_avec_gyroscope()){
+        Gyro_Init();
+    }
+
+    stdio_flush();
+    Trajet_config(100, 500);
+
+    multicore_launch_core1(affichage_test_evitement);
+
+    temps_ms = Temps_get_temps_ms();
+    temps_ms_init = temps_ms;
+    do{
+        i2c_gestion(i2c0);
+        i2c_annexe_gestion();
+        Balise_VL53L1X_gestion();
+        
+        // Routines à 1 ms
+        if(temps_ms != Temps_get_temps_ms()){
+            temps_ms = Temps_get_temps_ms();
+            QEI_update();
+            Localisation_gestion();
+            AsserMoteur_Gestion(_step_ms);
+
+            // Routine à 2 ms
+            if(temps_ms % _step_ms_gyro == 0){
+                if(get_position_avec_gyroscope()){
+                    Gyro_Read(_step_ms_gyro);
+                }
+            }
+
+            Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 
+                                1000,0,
+                                0, 0); // Angles
+
+            if(parcourt_trajet_simple(trajectoire, _step_ms) == ACTION_TERMINEE){
+                etat_strategie = CALAGE_PANIER_1;
+            }
+
+        }
+        lettre = getchar_timeout_us(0);
+    //}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
+    }while(1);
+    printf("STRATEGIE_LOOP_2\n");
+    printf("Lettre : %d; %c\n", lettre, lettre);
+    
+    if(lettre == 'q' && lettre == 'Q'){
+        return 0;
+    }
+    return 0;
+
+}
+
 
 int test_longe(){
     int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;