double => float + optimisation du temps de cycle

Holonome2023.c

@@ -2,6 +2,7 @@
 #include "pico/multicore.h"
 #include "pico/stdlib.h"
 #include "hardware/gpio.h"
+#include "hardware/i2c.h"
 #include "hardware/timer.h"
 #include "pico/binary_info.h"
 #include "math.h"
@@ -10,10 +11,15 @@
 #include "gyro.h"
 #include "Asser_Moteurs.h"
 #include "Asser_Position.h"
+#include "Balise_VL53L1X.h"
 #include "Commande_vitesse.h"
+#include "i2c_annexe.h"
+#include "i2c_maitre.h"
 #include "Localisation.h"
+#include "Monitoring.h"
 #include "Moteurs.h"
 #include "QEI.h"
+#include "Robot_config.h"
 #include "Servomoteur.h"
 #include "spi_nb.h"
 #include "Strategie.h"
@@ -41,6 +47,7 @@ int main() {
 
     uint32_t temps_ms = 0, temps_ms_old;
     uint32_t temps_us_debut_cycle;
+    uint32_t score=0;
 
     stdio_init_all();
 
@@ -75,64 +82,94 @@ int main() {
     AsserMoteur_Init();
     Localisation_init();
 
-    while(mode_test());
+    //while(mode_test());
-    //test_panier();
+    i2c_maitre_init();
-    //test_homologation();
+    Trajet_init();
 
-    Gyro_Init();
+    set_position_avec_gyroscope(1);
+    if(get_position_avec_gyroscope()){
+        Gyro_Init();
+    }
 
-    
+    multicore_launch_core1(Monitoring_display);
 
     temps_ms = Temps_get_temps_ms();
     temps_ms_old = temps_ms;
     while (1) { 
-        u_int16_t step_ms = 2;
+        static enum {
-        float coef_filtre = 1-0.8;
+            MATCH_ATTENTE_TIRETTE,
+            MATCH_EN_COURS,
+            MATCH_ARRET_EN_COURS,
+            MATCH_TERMINEE
+        } statu_match=MATCH_ATTENTE_TIRETTE;
+        uint16_t _step_ms_gyro = 2;
+        const uint16_t _step_ms = 1;
+        static uint32_t timer_match_ms=0;
+        static enum couleur_t couleur;
+
+        // Surveillance du temps d'execution
+        temps_cycle_check();
+
+        i2c_gestion(i2c0);
+        i2c_annexe_gestion();
+        Balise_VL53L1X_gestion();
         
-        while(temps_ms == Temps_get_temps_ms());
-        temps_ms = Temps_get_temps_ms();
-        temps_ms_old = temps_ms;
         
-        // Tous les pas de step_ms
+        if(temps_ms != Temps_get_temps_ms()){
-        if(!(temps_ms % step_ms)){
+            timer_match_ms = timer_match_ms + _step_ms;
-            temps_us_debut_cycle = time_us_32();
+            temps_ms = Temps_get_temps_ms();
-            Gyro_Read(step_ms);
+            QEI_update();
-            
+            Localisation_gestion();
-            //gyro_affiche(gyro_get_vitesse(), "Angle :");
+            AsserMoteur_Gestion(_step_ms);
-            // Filtre 
+
-            angle_gyro = gyro_get_vitesse();
+            // Routine à 2 ms
-            if(vitesse_filtre_x == V_INIT){
+            if(temps_ms % _step_ms_gyro == 0){
-                vitesse_filtre_x = angle_gyro.rot_x;
+                if(get_position_avec_gyroscope()){
-                vitesse_filtre_y = angle_gyro.rot_y;
+                    Gyro_Read(_step_ms_gyro);
-                vitesse_filtre_z = angle_gyro.rot_z;
+                }
-            }else{
-                vitesse_filtre_x = vitesse_filtre_x * (1-coef_filtre) + angle_gyro.rot_x * coef_filtre;
-                vitesse_filtre_y = vitesse_filtre_y * (1-coef_filtre) + angle_gyro.rot_y * coef_filtre;
-                vitesse_filtre_z = vitesse_filtre_z * (1-coef_filtre) + angle_gyro.rot_z * coef_filtre;
             }
-            temps_cycle = time_us_32() - temps_us_debut_cycle;
 
-            //printf("%#x, %#x\n", (float)temps_ms_old / 1000,  vitesse_filtre_z);
+            switch(statu_match){
+                case MATCH_ATTENTE_TIRETTE:
+                    if(lire_couleur() == COULEUR_VERT){
+                        // Tout vert
+                        i2c_annexe_couleur_balise(0b00011100, 0x0FFF);
+                    }else{
+                        // Tout bleu
+                        i2c_annexe_couleur_balise(0b00000011, 0x0FFF);
+                    }
+                    if(attente_tirette() == 0){
+                        statu_match = MATCH_EN_COURS;
+                        couleur = lire_couleur();
+                        timer_match_ms=0;
+                        i2c_annexe_couleur_balise(0, 0x00);
+                        score=5;
+                        i2c_annexe_envoie_score(score);
+                    }
+                    break;
 
-            //printf("%d, %d\n", temps_ms, (int32_t) (vitesse_filtre_z * 1000));
+                case MATCH_EN_COURS:
-            //gyro_affiche(angle_gyro, "Vitesse (°/s),");
+                    if (timer_match_ms > 98000){
-            
+                        printf("MATCH_ARRET_EN_COURS\n");
-        }
+                        statu_match = MATCH_ARRET_EN_COURS;
+                    }
+                    Strategie(couleur, _step_ms);
+                    break;
+                
+                case MATCH_ARRET_EN_COURS:
+                    commande_vitesse_stop();
+                    i2c_annexe_active_deguisement();
+                    if (timer_match_ms > 100000){
+                        statu_match = MATCH_TERMINEE;
+                    }
+                    
+                    break;
 
-        // Toutes les 50 ms
+                case MATCH_TERMINEE:
-        if((Temps_get_temps_ms() % 50) == 0){
+                    Moteur_Stop();
-            struct t_angle_gyro_float m_gyro;
+                    while(1);
-            m_gyro = gyro_get_angle_degres();
+                    break;
-            printf("%f, %f, %d\n", (float)temps_ms / 1000,  m_gyro.rot_z, temps_cycle);
+            }
-        }
-
-        // Toutes les 500 ms
-        if((Temps_get_temps_ms() % 500) == 0){
-            //gyro_affiche(gyro_get_angle(), "Angle :");
-        }
-        // Toutes les secondes
-        if((Temps_get_temps_ms() % 500) == 0){
-            //gyro_get_temp();
         }
     }
 }

Monitoring.c

@@ -1,9 +1,12 @@
 #include "pico/stdlib.h"
 #include <stdio.h>
+#include "Monitoring.h"
 
 uint32_t temps_cycle_min = UINT32_MAX;
 uint32_t temps_cycle_max=0;
 int lock=0;
+uint32_t debug_var=0;
+float debug_varf=0;
 
 void temps_cycle_check(){
     static uint32_t temps_old;
@@ -27,11 +30,17 @@ void temps_cycle_reset(){
     temps_cycle_max=0;
 }
 
+void Monitoring_display(){
+    temps_cycle_display();
+}
+
 void temps_cycle_display(){
     uint32_t temps;
     temps = time_us_32()/1000;
     printf(">T_cycle_min(us):%ld:%d\n", temps, temps_cycle_min);
     printf(">T_cycle_max(us):%ld:%d\n", temps, temps_cycle_max);
+    printf(">DebugVar:%ld:%d\n", temps, debug_var);
+    printf(">DebugVarf:%ld:%f\n", temps, debug_varf);
     temps_cycle_reset();
 }
 
@@ -42,3 +51,11 @@ uint32_t temps_cycle_get_min(){
 uint32_t temps_cycle_get_max(){
     return temps_cycle_max;
 }
+
+void set_debug_var(uint32_t variable){
+    debug_var = variable;
+}
+
+void set_debug_varf(float variable){
+    debug_varf = variable;
+}

Monitoring.h

@@ -4,4 +4,7 @@ void temps_cycle_check();
 void temps_cycle_reset();
 void temps_cycle_display();
 uint32_t temps_cycle_get_min();
-uint32_t temps_cycle_get_max();
+uint32_t temps_cycle_get_max();
+void set_debug_var(uint32_t variable);
+void set_debug_varf(float variable);
+void Monitoring_display();

Strategie.c

@@ -19,10 +19,114 @@
 // TODO: Peut-être à remetttre en variable locale après
 float distance_obstacle;
 
+enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t step_ms);
 enum etat_action_t calage_angle(enum longer_direction_t longer_direction, float x_mm, float y_mm, float angle_radian);
 enum etat_action_t lance_balles(uint32_t step_ms);
 
-enum etat_strategie_t etat_strategie=STRATEGIE_INIT;
+enum etat_homologation_t etat_homologation=STRATEGIE_INIT;
+
+
+void Strategie(enum couleur_t couleur, uint32_t step_ms){
+    
+    float angle_fin;
+    float recal_pos_x, recal_pos_y;
+    enum longer_direction_t longer_direction;
+    enum etat_action_t etat_action;
+    enum etat_trajet_t etat_trajet;
+
+    struct trajectoire_t trajectoire;
+
+    static enum etat_strategie_t {
+        STRATEGIE_INIT,
+        ALLER_CERISE_BAS,
+        ATTRAPER_CERISE_BAS,
+        ALLER_CERISE_HAUT,
+        ATTRAPER_CERISE_HAUT,
+        ALLER_CERISE_GAUCHE,
+        ATTRAPER_CERISE_GAUCHE,
+        ALLER_CERISE_DROITE,
+        ATTRAPER_CERISE_DROITE,
+        ALLER_PANIER,
+        LANCER_PANIER,
+    }etat_strategie;
+
+    switch(etat_strategie){
+        case STRATEGIE_INIT:
+            if(couleur == COULEUR_BLEU){
+                Localisation_set(775., 109., (-60.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN);
+            }else{
+                Localisation_set(2000 - 775., 109., (-60.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN);
+            }
+            etat_strategie = ALLER_CERISE_BAS;
+            break;
+
+        case ALLER_CERISE_BAS:
+            
+            if(couleur == COULEUR_BLEU){
+                angle_fin = 30. * DEGRE_EN_RADIAN;
+            }else{
+                angle_fin = -150. * DEGRE_EN_RADIAN;
+            }
+
+            Trajet_config(250, 500);            
+            Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, 857, 156,
+                     Localisation_get().angle_radian, angle_fin);
+
+            if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == TRAJET_TERMINE){
+                etat_strategie = ATTRAPER_CERISE_BAS;
+            }
+            break;
+
+        case ATTRAPER_CERISE_BAS:
+            recal_pos_y = RAYON_ROBOT;
+            if(couleur == COULEUR_BLEU){
+                longer_direction = LONGER_VERS_C;
+                recal_pos_x = 1000 - 15 - PETIT_RAYON_ROBOT_MM;
+            }else{
+                longer_direction = LONGER_VERS_A;
+                recal_pos_x = 1000 + 15 + PETIT_RAYON_ROBOT_MM;
+            }
+
+            etat_action = cerise_attraper_bordure(longer_direction, step_ms, recal_pos_x, recal_pos_y);
+            if(etat_action == ACTION_TERMINEE){
+                etat_strategie = ALLER_PANIER;
+            }
+            break;
+            
+        case ALLER_PANIER:
+            Trajet_config(500, 500);
+            if(couleur == COULEUR_BLEU){
+                Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 
+                                    485, Localisation_get().y_mm,
+                                    465, 857,
+                                    465,2830,
+                                    +30. * DEGRE_EN_RADIAN, +120. * DEGRE_EN_RADIAN);
+            }else{
+                Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 
+                                    485, Localisation_get().y_mm,
+                                    2000-465, 857,
+                                    2000-465,2830,
+                                    -150. * DEGRE_EN_RADIAN, -240. * DEGRE_EN_RADIAN);
+            }
+
+            if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
+                etat_strategie = LANCER_PANIER;
+            }
+            break;
+
+        case LANCER_PANIER:
+            if(lance_balles_dans_panier(couleur, step_ms)== ACTION_TERMINEE){
+                etat_homologation = STRATEGIE_FIN;
+            }
+            break;
+
+        case STRATEGIE_FIN:
+            i2c_annexe_desactive_propulseur();
+            commande_vitesse_stop();
+            break;
+    }
+}
+
 
 void Homologation(uint32_t step_ms){
     
@@ -32,15 +136,15 @@ void Homologation(uint32_t step_ms){
 
     struct trajectoire_t trajectoire;
 
-    switch(etat_strategie){
+    switch(etat_homologation){
         case STRATEGIE_INIT:
             Localisation_set(775., 109., (-60.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN);
-            etat_strategie = ATTENTE_TIRETTE;
+            etat_homologation = ATTENTE_TIRETTE;
             break;
 
         case ATTENTE_TIRETTE:
             if(attente_tirette() == 0){
-                etat_strategie = APPROCHE_CERISE_1_A;
+                etat_homologation = APPROCHE_CERISE_1_A;
             }
             break;
 
@@ -48,14 +152,14 @@ void Homologation(uint32_t step_ms){
             Trajet_config(250, 500);
             Trajectoire_droite(&trajectoire,775, 109, 857, 156, -60. * DEGRE_EN_RADIAN, 30. * DEGRE_EN_RADIAN);
             if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == TRAJET_TERMINE){
-                etat_strategie = ATTRAPE_CERISE_1;
+                etat_homologation = ATTRAPE_CERISE_1;
             }
             break;
         
         case ATTRAPE_CERISE_1:
-            etat_action = cerise_attraper_bordure(LONGER_VERS_C, step_ms, 1000-15-PETIT_RAYON_ROBOT_MM);
+            etat_action = cerise_attraper_bordure(LONGER_VERS_C, step_ms, 1000-15-PETIT_RAYON_ROBOT_MM, RAYON_ROBOT);
             if(etat_action == ACTION_TERMINEE){
-                etat_strategie = APPROCHE_PANIER_1;
+                etat_homologation = APPROCHE_PANIER_1;
             }
             break;
 
@@ -68,24 +172,13 @@ void Homologation(uint32_t step_ms){
                                 +30. * DEGRE_EN_RADIAN, +120. * DEGRE_EN_RADIAN);
 
             if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
-                etat_strategie = CALAGE_PANIER_1;
+                etat_homologation = CALAGE_PANIER_1;
             }
             break;
 
-        case APPROCHE_PANIER_2:
-            Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 
-                                265,2830,
-                                +120. * DEGRE_EN_RADIAN, +120. * DEGRE_EN_RADIAN);
-
-            if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
-                etat_strategie = CALAGE_PANIER_1;
-            }
-
-            break;
-
         case CALAGE_PANIER_1:
             if(calage_angle(LONGER_VERS_A, RAYON_ROBOT, 3000 - (RAYON_ROBOT/(RACINE_DE_3/2.)), 120. *DEGRE_EN_RADIAN) == ACTION_TERMINEE){
-                etat_strategie = RECULE_PANIER;
+                etat_homologation = RECULE_PANIER;
             }
             break;
         
@@ -96,14 +189,14 @@ void Homologation(uint32_t step_ms){
                                 120. * DEGRE_EN_RADIAN, +270. * DEGRE_EN_RADIAN);
 
             if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){
-                etat_strategie = LANCE_DANS_PANIER;
+                etat_homologation = LANCE_DANS_PANIER;
             }
             break;
 
         case LANCE_DANS_PANIER:
             Asser_Position_maintien();
             if(lance_balles(step_ms) == ACTION_TERMINEE){
-                etat_strategie = STRATEGIE_FIN;
+                etat_homologation = STRATEGIE_FIN;
             }
             break;
 
@@ -114,14 +207,14 @@ void Homologation(uint32_t step_ms){
                                 Localisation_get().angle_radian, 30. * DEGRE_EN_RADIAN);
 
             if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){
-                etat_strategie = STRATEGIE_FIN;
+                etat_homologation = STRATEGIE_FIN;
             }
             break;
 
         case ATTRAPPE_CERISE_2:
-            etat_action = cerise_attraper_bordure(LONGER_VERS_A, step_ms, 1000-15-PETIT_RAYON_ROBOT_MM);
+            etat_action = cerise_attraper_bordure(LONGER_VERS_A, step_ms, 1000-15-PETIT_RAYON_ROBOT_MM, 3000-RAYON_ROBOT);
             if(etat_action == ACTION_TERMINEE){
-                etat_strategie = APPROCHE_PANIER_2;
+                etat_homologation = APPROCHE_PANIER_2;
             }
             break;
 
@@ -134,6 +227,64 @@ void Homologation(uint32_t step_ms){
 }
 
 
+enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t step_ms){
+    static enum {
+        CALAGE_PANIER_1,
+        RECULE_PANIER,
+        LANCE_DANS_PANIER,
+    }etat_lance_balles_dans_panier;
+    float recal_pos_x, recal_pos_y;
+    enum longer_direction_t longer_direction;
+    float point_x, point_y;
+
+    enum etat_action_t etat_action = ACTION_EN_COURS;
+
+    struct trajectoire_t trajectoire;
+
+    switch(etat_lance_balles_dans_panier){
+        case CALAGE_PANIER_1:
+            if(couleur == COULEUR_BLEU){
+                recal_pos_x = RAYON_ROBOT;
+                longer_direction = LONGER_VERS_A;
+            }else{
+                recal_pos_x = 2000- RAYON_ROBOT;
+                longer_direction = LONGER_VERS_C;
+            }
+
+            if(calage_angle(longer_direction, recal_pos_x, 3000 - (RAYON_ROBOT/(RACINE_DE_3/2.)), 120. *DEGRE_EN_RADIAN) == ACTION_TERMINEE){
+                etat_lance_balles_dans_panier = RECULE_PANIER;
+            }
+            break;
+        
+        case RECULE_PANIER:
+            Trajet_config(250, 500);
+            if(couleur == COULEUR_BLEU){
+                point_x = 180;
+                point_y = 3000 - (RAYON_ROBOT/(RACINE_DE_3/2)) - 80;
+            }else{
+                point_x = 1735;
+                point_y = 3000 - (RAYON_ROBOT/(RACINE_DE_3/2)) - 80;
+            }
+            Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, 
+                                point_x, point_y,
+                                120. * DEGRE_EN_RADIAN, +270. * DEGRE_EN_RADIAN);
+
+            if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){
+                etat_lance_balles_dans_panier = LANCE_DANS_PANIER;
+            }
+            break;
+
+        case LANCE_DANS_PANIER:
+            Asser_Position_maintien();
+            if(lance_balles(step_ms) == ACTION_TERMINEE){
+                etat_action=ACTION_TERMINEE;
+            }
+            break;
+    }
+    return etat_action;
+
+}
+
 /// @brief Active le propulseur, ouvre la porte, attend qql secondes.
 /// @param step_ms : pas de temps.
 /// @return ACTION_EN_COURS ou ACTION_TERMINEE
@@ -185,7 +336,7 @@ enum etat_action_t lance_balles(uint32_t step_ms){
             if(temporisation_terminee(&tempo_ms, step_ms)){
                 i2c_annexe_mi_ferme_porte();
                 etat_lance_balle = LANCE_TEMPO_PROP_ON;
-                tempo_ms = 500;
+                tempo_ms = 750;
             }
             break;
 
@@ -237,7 +388,7 @@ enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint
             angle_avancement = Trajet_get_orientation_avance();
             distance_obstacle = Balise_VL53L1X_get_distance_obstacle_mm(angle_avancement);
             Trajet_set_obstacle_mm(distance_obstacle);
-            
+
             etat_trajet = Trajet_avance(step_ms/1000.);
             if(etat_trajet == TRAJET_TERMINE){
                 Trajet_set_obstacle_mm(DISTANCE_INVALIDE);

Strategie.h

@@ -25,7 +25,7 @@ enum couleur_t{
     COULEUR_INCONNUE,
 };
 
-extern enum etat_strategie_t{
+extern enum etat_homologation_t{
     STRATEGIE_INIT,
     ATTENTE_TIRETTE,
     APPROCHE_CERISE_1_A,
@@ -42,7 +42,7 @@ extern enum etat_strategie_t{
     APPROCHE_PANIER_3,
     STRATEGIE_FIN,
     
-}etat_strategie;
+}etat_homologation;
 
 enum etat_action_t cerise_accostage(void);
 enum etat_action_t avance_puis_longe_bordure(enum longer_direction_t longer_direction);
@@ -54,6 +54,7 @@ enum couleur_t lire_couleur(void);
 uint attente_tirette(void);
 enum etat_action_t lance_balles(uint32_t step_ms);
 int test_panier(void);
+void Strategie(enum couleur_t couleur, uint32_t step_ms);
 
 int temporisation_terminee(uint32_t * tempo_ms, uint32_t step_ms);
 

Strategie_prise_cerises.c

@@ -30,8 +30,11 @@ float vitesse_accostage_mm_s=100;
 /// @brief Fonction pour attraper les cerises sur les supports perpendiculaires à la bordure.
 /// Le robot accoste, longe le support cerise vers la bordure, active la turbine, puis longe le support cerise jusqu'à son bout.
 /// @param longer_direction : direction dans laquelle se trouve la bordure
+/// @param step_ms : fréquence d'appel
+/// @param pos_recalage_x_mm : Position de recalage contre le support de cerises
+/// @param pos_recalage_y_mm : Position de recalage contre la bordure du terrain
 /// @return ACTION_EN_COURS ou ACTION_TERMINEE
-enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direction, uint32_t step_ms, float pos_recalage_x_mm){
+enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direction, uint32_t step_ms, float pos_recalage_x_mm, float pos_recalage_y_mm){
     enum etat_action_t etat_action = ACTION_EN_COURS;
     enum longer_direction_t longer_direction_aspire;
     static uint32_t tempo_ms = 0;
@@ -55,6 +58,7 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct
             if( (longer_direction == LONGER_VERS_A) && (i2c_annexe_get_contacteur_butee_A() == CONTACTEUR_ACTIF) ||
                 (longer_direction == LONGER_VERS_C) && (i2c_annexe_get_contacteur_butee_C() == CONTACTEUR_ACTIF) ){
                     Localisation_set_x(pos_recalage_x_mm);
+                    Localisation_set_y(pos_recalage_y_mm);
                     etat_attrape = TURBINE_DEMARRAGE;
             }
             break;

Strategie_prise_cerises.h

@@ -1,2 +1,2 @@
 #include "Strategie.h"
-enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direction, uint32_t step_ms, float pos_x_mm);
+enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direction, uint32_t step_ms, float pos_x_mm, float pos_y_mm);

Test.c

@@ -1421,7 +1421,7 @@ void affiche_monitoring(){
 
 int test_angle_balise(void){
     int lettre;
-    float distance, angle=0;
+    float distance, angle=3.1281;
 
     i2c_maitre_init();
     Balise_VL53L1X_init();

Test_strategie.c

@@ -47,7 +47,10 @@ void affichage_test_strategie(){
 
         printf(">tirette:%ld:%d\n", temps, attente_tirette());
 
-        printf(">etat_strat:%ld:%d\n", temps, etat_strategie);
+        printf(">etat_strat:%ld:%d\n", temps, etat_homologation);
+        printf(">distance_obstacle:%ld:%f\n", temps, Trajet_get_obstacle_mm());
+        printf(">angle_avance:%ld:%f\n", temps, Trajet_get_orientation_avance());
+        
 
         /*switch(etat_strategie){
             case STRATEGIE_INIT: printf(">etat_strat:STRATEGIE_INIT|t\n"); break;
@@ -125,6 +128,7 @@ int test_homologation(){
 
     i2c_maitre_init();
     Trajet_init();
+    Balise_VL53L1X_init();
     //printf("Init gyroscope\n");
     set_position_avec_gyroscope(1);
     if(get_position_avec_gyroscope()){
@@ -191,6 +195,7 @@ void affichage_test_evitement(){
         for(uint8_t capteur=0; capteur<12; capteur++){
             printf(">c%d:%d\n",capteur, Balise_VL53L1X_get_capteur_cm(capteur));
         }
+        printf(">distance_minimale:%f\n",Trajet_get_obstacle_mm());
     }
 }
 

Trajectoire.c

@@ -3,11 +3,12 @@
 #include "Trajectoire_circulaire.h"
 #include "Trajectoire_droite.h"
 #include "Trajectoire_rotation.h"
+#include "Monitoring.h"
 
 #include "math.h"
 
 #define NB_MAX_TRAJECTOIRES 5
-#define PRECISION_ABSCISSE 0.001
+#define PRECISION_ABSCISSE 0.001f
 
 
 void Trajectoire_circulaire(struct trajectoire_t * trajectoire, float centre_x, float centre_y, float angle_debut_degre, float angle_fin_degre, float rayon,
@@ -115,7 +116,7 @@ float Trajectoire_get_longueur_mm(struct trajectoire_t * trajectoire){
 /// @brief Renvoie le point d'une trajectoire à partir de son abscisse
 /// @param abscisse : abscisse sur la trajectoire
 /// @return point en coordonnées X/Y
-struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, float abscisse){
+struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, double abscisse){
     struct point_xyo_t point_xyo;
     switch(trajectoire->type){
         case TRAJECTOIRE_DROITE:
@@ -149,8 +150,8 @@ float Trajectoire_get_orientation_rad(struct trajectoire_t * trajectoire, float
 /// @param abscisse : Valeur entre 0 et 1, position actuelle du robot sur sa trajectoire
 /// @param distance_mm : Distance en mm de laquelle le robot doit avancer sur la trajectoire
 /// @return nouvelle abscisse
-float Trajectoire_avance(struct trajectoire_t * trajectoire, float abscisse, float distance_mm){
+float Trajectoire_avance(struct trajectoire_t * trajectoire, double abscisse, double distance_mm){
-    float delta_abscisse, delta_mm, erreur_relative;
+    double delta_abscisse, delta_mm, erreur_relative;
 
     if(distance_mm == 0){
         return abscisse;
@@ -175,7 +176,7 @@ float Trajectoire_avance(struct trajectoire_t * trajectoire, float abscisse, flo
     return abscisse + delta_abscisse;
 }
 
-float distance_points(struct point_xy_t point, struct point_xy_t point_old){
+double distance_points(struct point_xy_t point, struct point_xy_t point_old){
     return sqrt( pow(point.x - point_old.x, 2) + pow(point.y - point_old.y , 2));
     
 }

Trajectoire.h

@@ -9,7 +9,7 @@ enum trajectoire_type_t{
 };
 
 struct point_xy_t{
-    float x, y;
+    double x, y;
 };
 
 struct point_xyo_t{
@@ -26,10 +26,10 @@ struct trajectoire_t {
 };
 
 float Trajectoire_get_longueur_mm(struct trajectoire_t * trajectoire);
-struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, float abscisse);
+struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, double abscisse);
 float Trajectoire_get_orientation_rad(struct trajectoire_t * trajectoire, float abscisse);
-float Trajectoire_avance(struct trajectoire_t * trajectoire, float abscisse, float distance_mm);
+float Trajectoire_avance(struct trajectoire_t * trajectoire, double abscisse, double distance_mm);
-float distance_points(struct point_xy_t point, struct point_xy_t point_old);
+double distance_points(struct point_xy_t point, struct point_xy_t point_old);
 void Trajectoire_circulaire(struct trajectoire_t * trajectoire, float centre_x, float centre_y, float angle_debut_degre, float angle_fin_degre,
                             float rayon, float orientation_debut_rad, float orientation_fin_rad);
 void Trajectoire_droite(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float p2_x, float p2_y, float orientation_debut_rad, float orientation_fin_rad);

Trajectoire_bezier.c

@@ -19,17 +19,17 @@ void Trajectoire_bezier_get_longueur(struct trajectoire_t * trajectoire){
 
 /// @brief Retourne le point sur la trajectoire en fonction de l'abscisse
 /// @param abscisse : compris entre 0 et 1
-struct point_xy_t Trajectoire_bezier_get_point(struct trajectoire_t * trajectoire, float abscisse){
+struct point_xy_t Trajectoire_bezier_get_point(struct trajectoire_t * trajectoire, double abscisse){
     struct point_xy_t point;
-    point.x = (float) trajectoire->p1.x * (1-abscisse) * (1-abscisse) * (1-abscisse) + 
+    point.x = (double) trajectoire->p1.x * (1-abscisse) * (1-abscisse) * (1-abscisse) + 
-        3 * (float) trajectoire->p2.x * abscisse * (1-abscisse) * (1-abscisse) +
+        3 * (double) trajectoire->p2.x * abscisse * (1-abscisse) * (1-abscisse) +
-        3 * (float) trajectoire->p3.x * abscisse * abscisse * (1-abscisse) +
+        3 * (double) trajectoire->p3.x * abscisse * abscisse * (1-abscisse) +
-            (float) trajectoire->p4.x * abscisse * abscisse * abscisse;
+            (double) trajectoire->p4.x * abscisse * abscisse * abscisse;
                     
-    point.y = (float) trajectoire->p1.y * (1-abscisse) * (1-abscisse) * (1-abscisse) + 
+    point.y = (double) trajectoire->p1.y * (1-abscisse) * (1-abscisse) * (1-abscisse) + 
-        3 * (float) trajectoire->p2.y * abscisse * (1-abscisse) * (1-abscisse) +
+        3 * (double) trajectoire->p2.y * abscisse * (1-abscisse) * (1-abscisse) +
-        3 * (float) trajectoire->p3.y * abscisse * abscisse * (1-abscisse) +
+        3 * (double) trajectoire->p3.y * abscisse * abscisse * (1-abscisse) +
-            (float) trajectoire->p4.y * abscisse * abscisse * abscisse;
+            (double) trajectoire->p4.y * abscisse * abscisse * abscisse;
 
     return point;
 }

Trajectoire_bezier.h

@@ -2,4 +2,4 @@
 
 
 void Trajectoire_bezier_get_longueur(struct trajectoire_t * trajectoire);
-struct point_xy_t Trajectoire_bezier_get_point(struct trajectoire_t * trajectoire, float abscisse);
+struct point_xy_t Trajectoire_bezier_get_point(struct trajectoire_t * trajectoire, double abscisse);

Trajet.c

@@ -3,6 +3,7 @@
 #include "Trajectoire.h"
 #include "Trajet.h"
 #include "Asser_Position.h"
+#include "Monitoring.h"
 
 float Trajet_calcul_vitesse(float temps_s);
 int Trajet_terminee(float abscisse);
@@ -60,6 +61,7 @@ enum etat_trajet_t Trajet_avance(float pas_de_temps_s){
 
     // Calcul de l'abscisse sur la trajectoire
     abscisse = Trajectoire_avance(&trajet_trajectoire, abscisse, distance_mm);
+    set_debug_varf(abscisse);
     
     // Obtention du point consigne
     point = Trajectoire_get_point(&trajet_trajectoire, abscisse);
@@ -126,14 +128,14 @@ float Trajet_calcul_vitesse(float pas_de_temps_s){
     distance_contrainte = Trajectoire_get_longueur_mm(&trajet_trajectoire) - position_mm;
     // 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);
+        vitesse_max_contrainte = sqrtf(2 * acceleration_mm_ss * distance_contrainte);
     }else{
         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);
+        vitesse_max_contrainte_obstacle = sqrtf(2 * acceleration_mm_ss_obstacle * distance_contrainte_obstacle);
         if(vitesse_max_contrainte_obstacle < vitesse_max_contrainte){
             vitesse_max_contrainte = vitesse_max_contrainte_obstacle;
         }
@@ -177,6 +179,10 @@ float Trajet_get_orientation_avance(){
     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);
+    angle = atan2f(point_suivant.point_xy.y - point.point_xy.y, point_suivant.point_xy.x - point.point_xy.x);
     return angle;
+}
+
+float Trajet_get_abscisse(){
+    return abscisse;
 }

Trajet.h

@@ -23,3 +23,4 @@ float Trajet_get_obstacle_mm(void);
 void Trajet_set_obstacle_mm(float distance_mm);
 void Trajet_stop(float);
 float Trajet_get_orientation_avance(void);
+float Trajet_get_abscisse();