evitement presque ok

Balise_VL53L1X.c

@@ -124,7 +124,7 @@ double Balise_VL53L1X_get_distance_obstacle_mm(double angle_avancement_radiant){
         double angle;
     } cone[NB_CONE];
     cone[0].angle = 22 * DEGRE_EN_RADIAN;
-    cone[0].distance_mm = 800;
+    cone[0].distance_mm = 1200;
 
     cone[1].angle = 50 * DEGRE_EN_RADIAN;
     cone[1].distance_mm = 580;
@@ -152,12 +152,13 @@ double Balise_VL53L1X_get_distance_obstacle_mm(double angle_avancement_radiant){
                 if(capteurs_VL53L1X[capteur].donnee_valide){
                     // Si la distance est plus petite que la distance minimale actuelle
                     // Si la distance est plus petite que le cône en question...
-                    if(capteurs_VL53L1X[capteur].distance_obstacle_robot_mm < distance_minimale /*&& 
+                    if(capteurs_VL53L1X[capteur].distance_obstacle_robot_mm < distance_minimale){
-                            capteurs_VL53L1X[capteur].distance_obstacle_robot_mm < cone[cone_index].distance_mm*/){
+                        if(capteurs_VL53L1X[capteur].distance_obstacle_robot_mm < cone[cone_index].distance_mm){
                             // On retient cette distance comme étant notre distance minimale
                             distance_minimale = capteurs_VL53L1X[capteur].distance_obstacle_robot_mm;
                         }
                     }
+                }
                 // Le capteur est pris en compte au moins dans un cône
                 capteurs_VL53L1X[capteur].donnee_ignore = 0;
             }

Geometrie.h

@@ -6,6 +6,7 @@
 #endif
 
 #define DEGRE_EN_RADIAN  (M_PI / 180.)
+#define DISTANCE_INVALIDE (-1.)
 
 struct position_t{
     double x_mm, y_mm;

Strategie.c

@@ -16,6 +16,9 @@
 #define DISTANCE_OBSTACLE_CM 50
 #define DISTANCE_PAS_OBSTACLE_MM 2000
 
+// TODO: Peut-être à remetttre en variable locale après
+double distance_obstacle;
+
 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);
 
@@ -201,6 +204,8 @@ enum etat_action_t calage_angle(enum longer_direction_t longer_direction, double
 enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint32_t step_ms){
     enum etat_action_t etat_action = ACTION_EN_COURS;
     enum etat_trajet_t etat_trajet;
+    double angle_avancement;
+    
     static enum {
         PARCOURS_INIT,
         PARCOURS_AVANCE,
@@ -213,12 +218,13 @@ 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){
+            angle_avancement = Trajet_get_orientation_avance();
-                Trajet_stop(step_ms);
+            distance_obstacle = Balise_VL53L1X_get_distance_obstacle_mm(angle_avancement);
-                break;
+            Trajet_set_obstacle_mm(distance_obstacle);
-            }
+            
             etat_trajet = Trajet_avance(step_ms/1000.);
             if(etat_trajet == TRAJET_TERMINE){
+                Trajet_set_obstacle_mm(DISTANCE_INVALIDE);
                 etat_action = ACTION_TERMINEE;
                 etat_parcourt = PARCOURS_INIT;
             }
@@ -239,6 +245,7 @@ enum etat_action_t parcourt_trajet_simple_sans_evitement(struct trajectoire_t tr
     switch (etat_parcourt){
         case PARCOURS_INIT:
             Trajet_debut_trajectoire(trajectoire);
+            Trajet_set_obstacle_mm(distance_pas_obstacle);
             etat_parcourt = PARCOURS_AVANCE;
             break;
         

Strategie.h

@@ -56,7 +56,7 @@ int test_panier(void);
 
 int temporisation_terminee(uint32_t * tempo_ms, uint32_t step_ms);
 
-
+extern double distance_obstacle;
 
 // STRATEGIE_H
 #endif

Test.c

@@ -1393,6 +1393,8 @@ int test_angle_balise(void){
         distance = Balise_VL53L1X_get_distance_obstacle_mm(angle);
         printf(">distance_obstacle:%3.0f\n", distance);
 
+        sleep_ms(100);
+
         lettre = getchar_timeout_us(0);
     }while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);
         

Test_strategie.c

@@ -67,6 +67,7 @@ void affichage_test_strategie(){
 int test_strategie(){
     printf("A - Accoster.\n");
     printf("C - Couleur et tirette.\n");
+    printf("E - Evitement\n");
     printf("H - Homologation.\n");
     printf("L - Longer.\n");
     printf("P - Panier.\n");
@@ -169,7 +170,6 @@ 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));
         }
@@ -218,11 +218,11 @@ int test_evitement(){
             }
 
             Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 
-                                1000,0,
+                                1000,200,
                                 0, 0); // Angles
 
             if(parcourt_trajet_simple(trajectoire, _step_ms) == ACTION_TERMINEE){
-                etat_strategie = CALAGE_PANIER_1;
+
             }
 
         }

Trajet.c

@@ -12,10 +12,12 @@ double position_mm; // Position en mm sur la trajectoire
 double vitesse_mm_s;
 double vitesse_max_trajet_mm_s=500;
 double acceleration_mm_ss;
+const double acceleration_mm_ss_obstacle = 1500;
 struct trajectoire_t trajet_trajectoire;
 struct position_t position_consigne;
 
 double distance_obstacle_mm;
+const double distance_pas_obstacle = 2000;
 
 /// @brief Initialise le module Trajet. A appeler en phase d'initilisation
 void Trajet_init(){
@@ -112,6 +114,7 @@ struct position_t Trajet_get_consigne(){
 /// @return vitesse déterminée en m/s
 double Trajet_calcul_vitesse(double pas_de_temps_s){
     double vitesse_max_contrainte;
+    double vitesse_max_contrainte_obstacle;
     double distance_contrainte,distance_contrainte_obstacle;
     double vitesse;
     // Calcul de la vitesse avec acceleration
@@ -120,13 +123,6 @@ double Trajet_calcul_vitesse(double pas_de_temps_s){
     // Calcul de la vitesse maximale due à la contrainte en fin de trajectoire (0 mm/s)
     // https://poivron-robotique.fr/Consigne-de-vitesse.html
     distance_contrainte = Trajectoire_get_longueur_mm(&trajet_trajectoire) - position_mm;
-/*
-    distance_contrainte_obstacle = Trajet_get_obstacle_mm();
-
-    if(distance_contrainte > distance_contrainte_obstacle){
-        distance_contrainte = distance_contrainte_obstacle;
-    }*/
-
     // En cas de dépassement, on veut garder la contrainte, pour l'instant
     if(distance_contrainte > 0){
         vitesse_max_contrainte = sqrt(2 * acceleration_mm_ss * distance_contrainte);
@@ -134,6 +130,15 @@ double Trajet_calcul_vitesse(double pas_de_temps_s){
         vitesse_max_contrainte = 0;
     }
 
+    distance_contrainte_obstacle = Trajet_get_obstacle_mm();
+    if(distance_contrainte_obstacle != DISTANCE_INVALIDE){
+        vitesse_max_contrainte_obstacle = sqrt(2 * acceleration_mm_ss_obstacle * distance_contrainte_obstacle);
+        if(vitesse_max_contrainte_obstacle < vitesse_max_contrainte){
+            vitesse_max_contrainte = vitesse_max_contrainte_obstacle;
+        }
+    }
+    
+
     // Selection de la vitesse la plus faible
     if(vitesse > vitesse_max_contrainte){
         vitesse = vitesse_max_contrainte;
@@ -152,3 +157,25 @@ double Trajet_get_obstacle_mm(void){
 void Trajet_set_obstacle_mm(double distance_mm){
     distance_obstacle_mm = distance_mm;
 }
+
+
+/// @brief Renvoi l'angle d'avancement du robot dans le référentiel du terrain
+/// @return angle en radian.
+double Trajet_get_orientation_avance(){
+    struct point_xyo_t point, point_suivant;
+    double avance_abscisse = 0.01;
+    double angle;
+
+    if(abscisse >= 1){
+        return 0;
+    }
+    if(abscisse + avance_abscisse >= 1){
+        avance_abscisse = 1 - abscisse;
+    }
+
+    point = Trajectoire_get_point(&trajet_trajectoire, abscisse);
+    point_suivant = Trajectoire_get_point(&trajet_trajectoire, abscisse + avance_abscisse);
+
+    angle = atan2(point_suivant.point_xy.y - point.point_xy.y, point_suivant.point_xy.x - point.point_xy.x);
+    return angle;
+}

Trajet.h

@@ -12,6 +12,7 @@ enum etat_trajet_t{
 // Vitesse et acceleration pour une rotation (rad/s et rad/s²)
 #define TRAJECT_CONFIG_ROTATION_PURE 2, 2
 
+extern const double distance_pas_obstacle;
 
 void Trajet_init();
 void Trajet_config(double _vitesse_max_trajet_mm_s, double _acceleration_mm_ss);
@@ -21,3 +22,4 @@ struct position_t Trajet_get_consigne(void);
 double Trajet_get_obstacle_mm(void);
 void Trajet_set_obstacle_mm(double distance_mm);
 void Trajet_stop(double);
+double Trajet_get_orientation_avance(void);