Homologation

.vscode/settings.json

@@ -3,6 +3,9 @@
         "timer.h": "c",
         "localisation.h": "c",
         "math.h": "c",
-        "strategie.h": "c"
+        "strategie.h": "c",
+        "trajectoire.h": "c",
+        "trajet.h": "c",
+        "asser_position.h": "c"
     }
 }

Balise_VL53L1X.c

@@ -59,11 +59,11 @@ void invalide_obstacle(struct capteur_VL53L1X_t *capteur_VL53L1X, struct positio
         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);
+    //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;
-    }*/
+    }
 
 }
 

Holonome2023.c

@@ -75,7 +75,8 @@ int main() {
     AsserMoteur_Init();
     Localisation_init();
 
-    while(mode_test());
+    //while(mode_test());
+    test_homologation();
 
     Gyro_Init();
 

Strategie.c

@@ -12,7 +12,8 @@
 
 #define SEUIL_RECAL_DIST_MM 75
 #define SEUIL_RECAL_ANGLE_RADIAN (5 * DEGREE_EN_RADIAN)
-#define DISTANCE_OBSTACLE_CM 35
+#define DISTANCE_OBSTACLE_CM 50
+#define DISTANCE_PAS_OBSTACLE_MM 2000
 
 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);
@@ -30,19 +31,19 @@ void Homologation(uint32_t step_ms){
     switch(etat_strategie){
         case STRATEGIE_INIT:
             Localisation_set(775., 109., -60. * DEGREE_EN_RADIAN);
-            etat_strategie = APPROCHE_CERISE_1_A;
+            etat_strategie = ATTENTE_TIRETTE;
+            break;
+
+        case ATTENTE_TIRETTE:
+            if(attente_tirette() == 0){
+                etat_strategie = APPROCHE_CERISE_1_A;
+            }
             break;
 
         case APPROCHE_CERISE_1_A:
             Trajet_config(250, 500);
             Trajectoire_droite(&trajectoire,775, 109, 857, 156, -60. * DEGREE_EN_RADIAN, +30. * DEGREE_EN_RADIAN);
-            Trajet_debut_trajectoire(trajectoire);
+            if(parcourt_trajet_simple(trajectoire, step_ms) == TRAJET_TERMINE){
-            etat_strategie = APPROCHE_CERISE_1_B;
-            break;
-
-        case APPROCHE_CERISE_1_B:
-            etat_trajet = Trajet_avance(step_ms/1000.);
-            if(etat_trajet == TRAJET_TERMINE){
                 etat_strategie = ATTRAPE_CERISE_1;
             }
             break;
@@ -105,6 +106,7 @@ void Homologation(uint32_t step_ms){
 
         case STRATEGIE_FIN:
             Moteur_Stop();
+            i2c_annexe_desactive_propulseur();
             break;
     }
 }
@@ -196,7 +198,7 @@ enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint
         
         case PARCOURS_AVANCE:
             if(Balise_VL53L1X_get_min_distance() <DISTANCE_OBSTACLE_CM){
-                Trajet_avance(0.);
+                Trajet_stop(step_ms);
                 break;
             }
             etat_trajet = Trajet_avance(step_ms/1000.);
@@ -212,7 +214,7 @@ enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint
 
 /// @brief Renvoi 1 si on doit attendre le déclenchement de la tirette
 uint attente_tirette(void){
-    return gpio_get(TIRETTE);
+    return !gpio_get(TIRETTE);
 }
 
 /// @brief Renvoi COULEUR_VERT ou COULEUR_BLEU

Strategie.h

@@ -24,16 +24,18 @@ enum couleur_t{
 };
 
 extern enum etat_strategie_t{
-    STRATEGIE_INIT=0,
+    STRATEGIE_INIT,
-    APPROCHE_CERISE_1_A=1,
+    ATTENTE_TIRETTE,
-    APPROCHE_CERISE_1_B=2,
+    APPROCHE_CERISE_1_A,
-    ATTRAPE_CERISE_1=3,
+    APPROCHE_CERISE_1_B,
-    APPROCHE_PANIER_1=4,
+    ATTRAPE_CERISE_1,
-    APPROCHE_PANIER_2=5,
+    APPROCHE_PANIER_1,
-    CALAGE_PANIER_1=6,
+    APPROCHE_PANIER_2,
-    RECULE_PANIER=7,
+    CALAGE_PANIER_1,
-    LANCE_DANS_PANIER=8,
+    RECULE_PANIER,
-    STRATEGIE_FIN=254,
+    TOURNE_PANIER,
+    LANCE_DANS_PANIER,
+    STRATEGIE_FIN,
 }etat_strategie;
 
 enum etat_action_t cerise_accostage(void);

Test_strategie.c

@@ -22,6 +22,7 @@ int test_longe(void);
 int test_homologation(void);
 int test_evitement(void);
 int test_tirette_et_couleur();
+void affichage_test_evitement();
 
 void affichage_test_strategie(){
     uint32_t temps;
@@ -41,6 +42,8 @@ void affichage_test_strategie(){
         printf(">c_pos_y:%ld:%f\n", temps, Trajet_get_consigne().y_mm);
         printf(">c_pos_angle:%ld:%f\n", temps, Trajet_get_consigne().angle_radian);
 
+        printf(">tirette:%ld:%d\n", temps, attente_tirette());
+
         printf(">etat_strat:%d\n",etat_strategie);
 
         /*switch(etat_strategie){
@@ -126,6 +129,7 @@ int test_homologation(){
     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();

Trajet.c

@@ -15,6 +15,8 @@ double acceleration_mm_ss;
 struct trajectoire_t trajet_trajectoire;
 struct position_t position_consigne;
 
+double distance_obstacle_mm;
+
 /// @brief Initialise le module Trajet. A appeler en phase d'initilisation
 void Trajet_init(){
     abscisse = 0;
@@ -74,11 +76,20 @@ enum etat_trajet_t Trajet_avance(double pas_de_temps_s){
 
 }
 
+void Trajet_stop(double pas_de_temps_s){
+    vitesse_mm_s = 0;
+    Trajet_avance(0);
+}
+
 /// @brief Savoir si un trajet est terminé est trivial sauf pour les courbes de Bézier
 /// où les approximations font que l'abscisse peut ne pas atteindre 1.
 /// @param abscisse : abscisse sur la trajectoire
 /// @return 1 si le trajet est terminé, 0 sinon
 int Trajet_terminee(double abscisse){
+    if(abscisse >= 0.99 ){
+        return 1;
+    }
+    /*
     if(trajet_trajectoire.type != TRAJECTOIRE_BEZIER){
         if(abscisse >= 1 ){
             return 1;
@@ -87,7 +98,7 @@ int Trajet_terminee(double abscisse){
         if(abscisse >= 0.99 ){
             return 1;
         }
-    }
+    }*/
     return 0;
 }
 
@@ -101,7 +112,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 distance_contrainte;
+    double distance_contrainte,distance_contrainte_obstacle;
     double vitesse;
     // Calcul de la vitesse avec acceleration
     vitesse = vitesse_mm_s + acceleration_mm_ss * pas_de_temps_s;
@@ -109,6 +120,13 @@ 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);
@@ -125,3 +143,12 @@ double Trajet_calcul_vitesse(double pas_de_temps_s){
     }
     return vitesse;
 }
+
+
+double Trajet_get_obstacle_mm(void){
+    return distance_obstacle_mm;
+}
+
+void Trajet_set_obstacle_mm(double distance_mm){
+    distance_obstacle_mm = distance_mm;
+}

Trajet.h

@@ -18,3 +18,6 @@ void Trajet_config(double _vitesse_max_trajet_mm_s, double _acceleration_mm_ss);
 void Trajet_debut_trajectoire(struct trajectoire_t trajectoire);
 enum etat_trajet_t Trajet_avance(double temps_s);
 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);