Distinction du cas où la localisation se fait sans gyroscope et celui où le gyroscope est en erreur

CMakeLists.txt

@@ -23,6 +23,7 @@ gyro_ADXRS453.c
 i2c_maitre.c
 i2c_annexe.c
 Localisation.c
+Log.c
 Moteurs.c
 Monitoring.c
 Robot_config.c
@@ -31,8 +32,12 @@ Servomoteur.c
 Strategie.c
 Strategie_deplacement.c
 Strategie_prise_cerises.c
+Strategie_pousse_gateau.c
 Temps.c
 Test.c
+Test_gyro.c
+Test_i2c.c
+Test_log.c
 Test_strategie.c
 Trajet.c
 Trajectoire.c
@@ -44,7 +49,8 @@ spi_nb.c)
 
 pico_generate_pio_header(test ${CMAKE_CURRENT_LIST_DIR}/quadrature_encoder.pio)
 
-add_definitions(-DGYRO_ADXRS453)
+add_definitions(-DGYRO_ADXRS453=1)
+add_definitions(-DPICO_MALLOC_PANIC=0)
 pico_enable_stdio_usb(test 1)
 pico_enable_stdio_uart(test 1)
 pico_add_extra_outputs(test)

Holonome2023.c

@@ -41,6 +41,12 @@ uint temps_cycle;
 
 int mode_test();
 
+void init_led(uint Numero_de_la_led, uint etat){
+    gpio_init(Numero_de_la_led);
+    gpio_set_dir(Numero_de_la_led, GPIO_OUT);
+    gpio_put(Numero_de_la_led, etat);
+}
+
 int main() {
     bi_decl(bi_program_description("This is a test binary."));
     bi_decl(bi_1pin_with_name(LED_PIN, "On-board LED"));
@@ -54,13 +60,8 @@ int main() {
 
     stdio_init_all();
 
-    gpio_init(LED_PIN);
-    gpio_set_dir(LED_PIN, GPIO_OUT);
-    gpio_put(LED_PIN, 1);
-
-    gpio_init(LED_PIN_ROUGE);
-    gpio_set_dir(LED_PIN_ROUGE, GPIO_OUT);
-    gpio_put(LED_PIN_ROUGE, 0);
+    init_led(LED_PIN, 1);
+    init_led(LED_PIN_ROUGE, 0);
 
     gpio_init(COULEUR);
     gpio_init(TIRETTE);
@@ -85,7 +86,7 @@ int main() {
     AsserMoteur_Init();
     Localisation_init();
 
-    //while(mode_test());
+    while(mode_test());
     i2c_maitre_init();
     Trajet_init();
     Balise_VL53L1X_init();
@@ -161,7 +162,7 @@ int main() {
                     break;
 
                 case MATCH_EN_COURS:
-                    if (timer_match_ms > 98000){
+                    if (timer_match_ms > 98000){ // 98 secondes
                         printf("MATCH_ARRET_EN_COURS\n");
                         statu_match = MATCH_ARRET_EN_COURS;
                     }
@@ -176,7 +177,7 @@ int main() {
                         Score_set_pieds_dans_plat();
                     }
 
-                    if (timer_match_ms > 100000){
+                    if (timer_match_ms > 100000){ // 100 secondes
                         statu_match = MATCH_TERMINEE;
                     }
                     

Localisation.c

@@ -45,7 +45,7 @@ void Localisation_gestion(){
     delta_x_ref_robot = (distance_roue_a_mm + distance_roue_b_mm - 2 * distance_roue_c_mm)  / 3.0;
     delta_y_ref_robot = (-distance_roue_a_mm + distance_roue_b_mm)  * RACINE_DE_3 / 3.0;
 
-    if(get_position_avec_gyroscope()){
+    if(get_position_avec_gyroscope() && !get_position_avec_gyroscope_error()){
         angle_gyro = gyro_get_angle_degres();
         position.angle_radian = angle_gyro.rot_z / 180. * M_PI ;
     }else{

Log.c

@@ -0,0 +1,86 @@
+#include "Log.h"
+#include "string.h"
+#include <stdio.h>
+#include <stdlib.h>
+
+struct log_message_storage{
+    struct Log_message_data message;
+    struct log_message_storage * next;
+} * log_message_envoi, log_message_storage_premier, *log_message_storage_courant;
+
+uint log_error = 0;
+enum Log_level log_level_reference;
+
+int store_new_message(struct Log_message_data message, struct log_message_storage * stockage);
+void envoi_message(struct Log_message_data message);
+
+void Log_init(void){
+    log_message_storage_premier.message.log_level = INFO;
+    strcpy(log_message_storage_premier.message.message,"Debut du programme");
+    log_message_storage_premier.message.timestamp = time_us_32() / 1000;
+    log_message_storage_premier.next = NULL;
+
+    log_message_storage_courant = &log_message_storage_premier;
+    log_message_envoi = &log_message_storage_premier;
+
+    log_level_reference = TELEPLOT;
+}
+
+
+/// @brief Add one log message to local cache. Should be quick
+/// @param message : string, without '\n' at the end.
+/// @param log_level : can be in TELEPLOT, TRACE, DEBUG, INFO, WARN, ERROR, FATAL
+void Log_message(char * message, enum Log_level log_level){
+    // On vérifie que le message a une urgence suffisante pour être enregistrée
+    if(log_level >= log_level_reference){
+        /// Création de la structure de données
+        struct Log_message_data message_container;
+        if(strlen(message) > LOG_MAX_MESSAGE_SIZE){
+            strcpy(message_container.message, "MSG TOO LONG");
+        }else{
+            strcpy(message_container.message, message);
+        }
+        message_container.log_level = log_level;
+        message_container.timestamp = time_us_32() / 1000;
+
+        /// Insertion de la structure dans la liste chaînée
+        struct log_message_storage* tmp_message_storage;
+        tmp_message_storage = (struct log_message_storage*) malloc(sizeof(struct log_message_storage));
+        if(tmp_message_storage != NULL){
+            tmp_message_storage->message = message_container;
+            tmp_message_storage->next = NULL;
+            log_message_storage_courant->next = tmp_message_storage;
+            log_message_storage_courant = log_message_storage_courant->next;
+        }else{
+            log_error |= LOG_ERROR_MEMORY_FULL;
+        }
+    }
+}
+
+/// @brief Read messages in cache, store them and send them through the serial connection
+void Log_gestion(){
+    // Envoi 1 message par la liaison série
+    if(log_message_envoi->next != NULL){
+        log_message_envoi = log_message_envoi->next;
+        envoi_message(log_message_envoi->message);
+        // Si on est à la fin des messages, en envoie le statut de la fonction Log
+        if(log_message_envoi->next == NULL){
+            if(log_error & LOG_ERROR_MEMORY_FULL){
+                printf("LOG ERROR : Memoire pleine !\n");
+            }
+        }
+    }
+}
+
+/// @brief Renvoie l'intégralité du journal de log. La fonction Log_gestion() doit être appelée continuellement pour que cette fonction fonctionne.
+void Log_get_full_log(){
+    log_message_envoi = &log_message_storage_premier;
+}
+
+void envoi_message(struct Log_message_data message_data){
+    if(message_data.log_level == TELEPLOT){
+        printf("%s\n", message_data.message);
+    }else{
+        printf("%u ms:%s\n", message_data.timestamp, message_data.message);
+    }
+}

Log.h

@@ -0,0 +1,20 @@
+#include "pico/stdlib.h"
+
+#define LOG_MAX_MESSAGE_SIZE 64
+
+#define LOG_ERROR_MEMORY_FULL 1
+
+enum Log_level{
+    TELEPLOT, TRACE, DEBUG, INFO, WARN, ERROR, FATAL
+};
+
+struct Log_message_data{
+    char message[LOG_MAX_MESSAGE_SIZE];
+    enum Log_level log_level;
+    uint32_t timestamp;
+};
+
+void Log_init(void);
+void Log_gestion(void);
+void Log_message(char * message, enum Log_level log_level);
+void Log_get_full_log();

Robot_config.c

@@ -1,4 +1,5 @@
 int position_avec_gyroscope = 0;
+int position_avec_gyroscope_erreur = 0;
 
 int get_position_avec_gyroscope(void){
     return position_avec_gyroscope;
@@ -7,3 +8,15 @@ int get_position_avec_gyroscope(void){
 void set_position_avec_gyroscope(int _use_gyro){
     position_avec_gyroscope = _use_gyro;
 }
+
+/// @brief Indique que le gyroscope est en erreur
+/// @param _erreur : 1 si en erreur
+void set_position_avec_gyroscope_error(int _erreur){
+    position_avec_gyroscope_erreur = _erreur;
+}
+
+/// @brief Indique si le gyroscope est en erreur
+/// @return 1 si en erreur, 0 sinon
+int get_position_avec_gyroscope_error(){
+    return position_avec_gyroscope_erreur;
+}

Robot_config.h

@@ -1,2 +1,4 @@
 int get_position_avec_gyroscope(void);
 void set_position_avec_gyroscope(int _use_gyro);
+void set_position_avec_gyroscope_error(int _erreur);
+int get_position_avec_gyroscope_error(void);

Score.c

@@ -7,6 +7,7 @@
 uint32_t Score_nb_cerises=0;
 uint32_t Score_nb_points=0;
 uint32_t Score_funny_action=0;
+uint32_t Score_gateau=0;
 uint32_t Score_pieds_dans_plat=0;
 
 void Score_recalcule(){
@@ -16,6 +17,7 @@ void Score_recalcule(){
     }
     Score_nb_points += Score_funny_action;
     Score_nb_points += Score_pieds_dans_plat;
+    Score_nb_points += Score_gateau;
 
 }
 
@@ -24,6 +26,12 @@ void Score_actualise(){
     i2c_annexe_envoie_score(Score_nb_points);
 }
 
+void Score_ajout_gateau(uint32_t nb_gateau){
+    Score_gateau += nb_gateau;
+    Score_actualise();
+}
+
+
 void Score_set_funny_action(){
     Score_funny_action = 5;
     Score_actualise();

Score.h

@@ -3,3 +3,4 @@
 void Score_set_funny_action();
 void Score_set_pieds_dans_plat();
 void Score_ajout_cerise(uint32_t nb_cerises);
+void Score_ajout_gateau(uint32_t nb_gateau);

Strategie.c

@@ -53,7 +53,7 @@ enum etat_action_t Strategie_aller_cerises_laterales_opposees(enum couleur_t cou
 
 
 void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
-    const uint32_t temps_pre_fin_match = (97000 - 15000);
+    const uint32_t temps_pre_fin_match = (97000 - 10000);
     static bool pre_fin_match_active=false;
     float angle_fin;
     float recal_pos_x, recal_pos_y;
@@ -96,9 +96,9 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
         case STRATEGIE_INIT:
             if(couleur == COULEUR_BLEU){
                 Localisation_set(225., 3000 - PETIT_RAYON_ROBOT_MM, (120.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN);
-                struct objectif_t objectif_1 = { .priorite = 10, .etat = FAIT, .cible = CERISE_BAS};
-                struct objectif_t objectif_2 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_HAUT};
-                struct objectif_t objectif_3 = { .priorite = 2, .etat = A_FAIRE, .cible = CERISE_GAUCHE};
+                struct objectif_t objectif_1 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_HAUT};
+                struct objectif_t objectif_2 = { .priorite = 2, .etat = A_FAIRE, .cible = CERISE_GAUCHE};
+                struct objectif_t objectif_3 = { .priorite = 3, .etat = A_FAIRE, .cible = CERISE_BAS};
                 struct objectif_t objectif_4 = { .priorite = 5, .etat = FAIT, .cible = CERISE_DROITE};
                 objectifs[0]= objectif_1;
                 objectifs[1]= objectif_2;
@@ -106,16 +106,16 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
                 objectifs[3]= objectif_4;
             }else{
                 Localisation_set(2000 - 225., 3000 - PETIT_RAYON_ROBOT_MM, (120.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN);
-                struct objectif_t objectif_1 = { .priorite = 10, .etat = FAIT, .cible = CERISE_BAS};
-                struct objectif_t objectif_2 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_HAUT};
-                struct objectif_t objectif_3 = { .priorite = 2, .etat = A_FAIRE, .cible = CERISE_DROITE};
+                struct objectif_t objectif_1 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_HAUT};
+                struct objectif_t objectif_2 = { .priorite = 2, .etat = A_FAIRE, .cible = CERISE_DROITE};
+                struct objectif_t objectif_3 = { .priorite = 3, .etat = A_FAIRE, .cible = CERISE_BAS};
                 struct objectif_t objectif_4 = { .priorite = 5, .etat = FAIT, .cible = CERISE_GAUCHE};
                 objectifs[0]= objectif_1;
                 objectifs[1]= objectif_2;
                 objectifs[2]= objectif_3;
                 objectifs[3]= objectif_4;
             }
-            objectif_courant = &objectifs[0];
+            objectif_courant = &objectifs[4];
             Strategie_set_cerise_dans_robot(10);
             etat_strategie = LANCER_PANIER;
             break;
@@ -124,18 +124,24 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
             
             if(couleur == COULEUR_BLEU){
                 angle_fin = 30. * DEGRE_EN_RADIAN;
-                point_x = 857;
+                angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin);
+                Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
+                    650, Localisation_get().y_mm,
+                    490, 0,
+                    857, 0,
+                    Localisation_get().angle_radian, angle_fin);
             }else{
                 angle_fin = -150. * DEGRE_EN_RADIAN;
-                point_x = 2000 - 857;
-            }
                 angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin);
-
-            Trajet_config(250, 250);            
-            Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 156,
+                Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
+                    2000 - 650, Localisation_get().y_mm,
+                    2000 - 490, 0,
+                    2000 - 857, 0,
                     Localisation_get().angle_radian, angle_fin);
+            }
+            Trajet_config(500,250);
 
-            if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == TRAJET_TERMINE){
+            if(Strategie_parcourir_trajet(trajectoire, step_ms, RETOUR_SI_OBSTABLE) == TRAJET_TERMINE){
                 etat_strategie = ATTRAPER_CERISE_BAS;
             }
             break;
@@ -168,7 +174,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
             }
             angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin);
 
-            Trajet_config(250, 250);            
+            Trajet_config(500, 250);            
             Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 175,
                      Localisation_get().angle_radian, angle_fin);
 
@@ -200,16 +206,13 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
             if(couleur == COULEUR_BLEU){
                 if(Strategie_aller_cerises_laterales_proches(couleur, step_ms)== ACTION_TERMINEE){
                     etat_strategie = ATTRAPER_CERISE_GAUCHE;
+                    Score_ajout_gateau(3);
                 }
             }else{
                 if(Strategie_aller_cerises_laterales_opposees(couleur, step_ms)== ACTION_TERMINEE){
                     etat_strategie = ATTRAPER_CERISE_GAUCHE;
                 }
             }
-
-            if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
-                etat_strategie = ATTRAPER_CERISE_GAUCHE;
-            }
             break;
 
         case ATTRAPER_CERISE_GAUCHE:
@@ -228,13 +231,10 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
                 }
             }else{
                 if(Strategie_aller_cerises_laterales_proches(couleur, step_ms)== ACTION_TERMINEE){
+                    Score_ajout_gateau(3);
                     etat_strategie = ATTRAPER_CERISE_DROITE;
                 }
             }
-
-            if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
-                etat_strategie = ATTRAPER_CERISE_GAUCHE;
-            }
             break;
 
         case ATTRAPER_CERISE_DROITE:
@@ -293,6 +293,8 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
             break;
         
         case RETOUR_MAISON:
+            i2c_annexe_plie_bras();
+            i2c_annexe_desactive_turbine();
             if(Strategie_pieds_dans_plat(couleur, step_ms) == ACTION_TERMINEE){
                 // Si le robot est dans la zone du panier, jeter les cerises s'il en a
                 if(Strategie_get_cerise_dans_robot() > 0 && Robot_est_dans_zone_depose_panier(couleur)){
@@ -315,9 +317,6 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
 }
 
 enum etat_action_t Strategie_aller_cerises_laterales_proches(enum couleur_t couleur, uint32_t step_ms){
-    struct trajectoire_t trajectoire;
-    float angle_fin;
-
     return Gateau_pousse_proche(couleur, step_ms);
 
     /*if(couleur == COULEUR_BLEU){
@@ -365,7 +364,7 @@ enum etat_action_t Strategie_aller_cerises_laterales_opposees(enum couleur_t cou
 enum etat_action_t  Strategie_aller_panier(enum couleur_t couleur, uint32_t step_ms){
     enum etat_action_t etat_action = ACTION_EN_COURS;
     struct trajectoire_t trajectoire;
-    Trajet_config(250, 250);
+    Trajet_config(TRAJECT_CONFIG_STD);
 
     // Definition des trajectoires
     if(couleur == COULEUR_BLEU){
@@ -385,10 +384,11 @@ enum etat_action_t  Strategie_aller_panier(enum couleur_t couleur, uint32_t step
                 Geometrie_get_angle_optimal(Localisation_get().angle_radian, +120. * DEGRE_EN_RADIAN));
         }else{
         // Sinon, on a une courbe de bézier
+            Trajet_config(500,250);
             Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 
-                485, Localisation_get().y_mm,
-                465, 857,
-                465,2830,
+                418, Localisation_get().y_mm,
+                655, 2800,
+                180, 2800,
                 Localisation_get().angle_radian, 
                 Geometrie_get_angle_optimal(Localisation_get().angle_radian, +120. * DEGRE_EN_RADIAN));
         }
@@ -408,17 +408,17 @@ enum etat_action_t  Strategie_aller_panier(enum couleur_t couleur, uint32_t step
                 Geometrie_get_angle_optimal(Localisation_get().angle_radian, -240. * DEGRE_EN_RADIAN));
         }else{
         // Sinon, on a une courbe de bézier
+            Trajet_config(500,250);
             Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 
-                            2000-485, Localisation_get().y_mm,
-                            2000-465, 857,
-                            2000-465, 2830,
+                2000 - 418, Localisation_get().y_mm,
+                2000 - 655, 2800,
+                2000 - 180, 2800,
                 Localisation_get().angle_radian, Geometrie_get_angle_optimal(Localisation_get().angle_radian, -240. * DEGRE_EN_RADIAN));
         }
-        
     }
 
     // parcours du trajet
-    if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
+    if(Strategie_parcourir_trajet(trajectoire, step_ms, RETOUR_SI_OBSTABLE) == ACTION_TERMINEE){
         etat_action = ACTION_TERMINEE;
     }
     return etat_action;

Strategie.h

@@ -62,6 +62,7 @@ enum etat_action_t Strategie_preparation();
 
 enum etat_action_t Strategie_pieds_dans_plat_trajet(struct objectif_t *objectif_plat_courant, enum couleur_t couleur, uint32_t step_ms);
 enum etat_action_t Strategie_pieds_dans_plat(enum couleur_t couleur, uint32_t step_ms);
+enum etat_action_t Strategie_aller_a(float pos_x, float pos_y, uint32_t step_ms);
 
 extern float distance_obstacle;
 

Strategie_deplacement.c

@@ -8,6 +8,7 @@ enum etat_action_t Strategie_parcourir_trajet(struct trajectoire_t trajectoire,
     enum etat_action_t etat_action = ACTION_EN_COURS;
     enum etat_trajet_t etat_trajet;
     float angle_avancement;
+    static bool trajet_inverse = false;
     
     static enum {
         PARCOURS_INIT,
@@ -41,6 +42,7 @@ enum etat_action_t Strategie_parcourir_trajet(struct trajectoire_t trajectoire,
                         return ACTION_ECHEC;
 
                     case RETOUR_SI_OBSTABLE:
+                        trajet_inverse = !trajet_inverse;
                         Trajet_inverse();
                         break;
 
@@ -53,7 +55,11 @@ enum etat_action_t Strategie_parcourir_trajet(struct trajectoire_t trajectoire,
 
             etat_trajet = Trajet_avance(step_ms/1000.);
             if(etat_trajet == TRAJET_TERMINE){
+                if(trajet_inverse){
+                    etat_action = ACTION_ECHEC;
+                }else{
                     etat_action = ACTION_TERMINEE;
+                }
                 etat_parcourt = PARCOURS_INIT;
             }
             break;

Strategie_pousse_gateau.c

@@ -1,5 +1,6 @@
 #include "i2c_annexe.h"
 #include "Localisation.h"
+#include "Monitoring.h"
 #include "Strategie.h"
 #include "Trajectoire.h"
 #include "Trajet.h"
@@ -7,57 +8,77 @@
 
 enum etat_action_t Gateau_pousse_proche(enum couleur_t couleur, uint32_t step_ms){
     static enum {
-        GATEAU_PROCHE_PRE_POUSSE,
-        GATEAU_PROCHE_POUSSE,
-        GATEAU_PROCHE_RELACHE,
-    } etat_pousse_proche;
-
+        ALLER_1,
+        ALLER_2,
+        ALLER_3
+    } etat=ALLER_1;
+    float point_x, point_y;
+    float angle_fin;
     struct trajectoire_t trajectoire;
     
-    float angle_fin, point_x, point_y;
     
-    switch(etat_pousse_proche){
-        case GATEAU_PROCHE_PRE_POUSSE:
+    switch(etat){
+        case ALLER_1:
             if(couleur == COULEUR_BLEU){
                 angle_fin = 90. * DEGRE_EN_RADIAN;
                 point_x = 310;
             }else{
-                angle_fin = -135. * DEGRE_EN_RADIAN;
-                point_x = 1655;
+                angle_fin = 205. * DEGRE_EN_RADIAN;
+                point_x = 1700;
             }
             angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin);
 
-            Trajet_config(250, 250);            
-            Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 360,
+            Trajet_config(100, 250);
+            Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 380,
                         Localisation_get().angle_radian, angle_fin);
 
-            if(parcourt_trajet_simple(trajectoire, step_ms) == TRAJET_TERMINE){
-                etat_pousse_proche = GATEAU_PROCHE_POUSSE;
+            if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
+                etat=ALLER_2;
                 i2c_annexe_deplie_bras();
             }
             break;
         
-        case GATEAU_PROCHE_POUSSE:
+        case ALLER_2:
             if(couleur == COULEUR_BLEU){
                 angle_fin = 90. * DEGRE_EN_RADIAN;
-                point_x = 390;
+                point_x = 310;
             }else{
-                angle_fin = -135. * DEGRE_EN_RADIAN;
-                point_x = 1585;
+                angle_fin = 205. * DEGRE_EN_RADIAN;
+                point_x = 1700;
+            }
+            angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin);
+
+            Trajet_config(500,250);
+            Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 1700,
+                        Localisation_get().angle_radian, angle_fin);
+
+            if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
+                i2c_annexe_plie_bras();
+                etat=ALLER_3;
+            }
+            break;
+        
+        case ALLER_3:
+            if(couleur == COULEUR_BLEU){
+                angle_fin = -150. * DEGRE_EN_RADIAN;
+                point_x = 225;
+            }else{
+                angle_fin = 30. * DEGRE_EN_RADIAN;
+                point_x = 1775;
             }
             angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin);
 
             Trajet_config(250, 250);
-            Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 1665,
+            Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 1600,
                         Localisation_get().angle_radian, angle_fin);
 
-            if(parcourt_trajet_simple(trajectoire, step_ms) == TRAJET_TERMINE){
-                etat_pousse_proche = GATEAU_PROCHE_POUSSE;
-                i2c_annexe_plie_bras();
+            if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
+                etat=ALLER_1;
+                return ACTION_TERMINEE;
             }
             break;
 
-        case GATEAU_PROCHE_RELACHE:
     }
+    return ACTION_EN_COURS;
 
 }

Strategie_prise_cerises.c

@@ -43,7 +43,7 @@ enum etat_action_t cerises_attraper_cerises_droite(uint32_t step_ms){
     }etat_attrappe_cerises_droite = ATTRAPE_CERISE_DEMI_BAS;
     struct trajectoire_t trajectoire;
     float angle_fin;
-    float pos_recal_x_mm = PETIT_RAYON_ROBOT_MM + 30;
+    float pos_recal_x_mm = 2000 - (PETIT_RAYON_ROBOT_MM + 30);
 
     switch (etat_attrappe_cerises_droite){
         case ATTRAPE_CERISE_DEMI_BAS:
@@ -56,7 +56,7 @@ enum etat_action_t cerises_attraper_cerises_droite(uint32_t step_ms){
             angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, 30 * DEGRE_EN_RADIAN);
 
             Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
-                    2000 - 180, 1500 - 75, Localisation_get().angle_radian, angle_fin);
+                    2000 - 180, 3000 - 1600, Localisation_get().angle_radian, angle_fin);
                     
             if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
                 etat_attrappe_cerises_droite = ATTRAPE_CERISE_DEMI_HAUT;

Test.c

@@ -28,6 +28,9 @@
 #include "Trajectoire.h"
 #include "Trajet.h"
 
+#include "Test_gyro.h"
+#include "Test_i2c.h"
+#include "Test_log.h"
 #include "Test_strategie.h"
 #include "Test.h"
 
@@ -51,13 +54,7 @@ int test_asser_position_avance(void);
 int test_asser_position_avance_et_tourne(int, int);
 int test_transition_gyro_pas_gyro(void);
 int test_trajectoire(void);
-int test_i2c_bus(void);
 void affiche_localisation(void);
-int test_i2c_lecture_pico_annex();
-int test_i2c_lecture_pico_annex_nb();
-int test_i2c_lecture_pico_annex_nb2();
-int test_i2c_ecriture_pico_annex_nb();
-int test_i2c_ecriture_pico_annex_nb_2();
 int test_aller_retour();
 void test_trajectoire_teleplot();
 int test_capteurs_balise(void);
@@ -86,13 +83,11 @@ int mode_test(){
     printf("O - Analyse obstacle\n");
     printf("P - Asser Position - perturbation\n");
     printf("Q - Asser Position - transition Gyro -> Pas gyro\n");
+    printf("R - Test des logs...\n");
+    printf("S - Test du gyroscope...\n");
     printf("T - Trajectoire\n");
-    printf("U - Scan du bus i2c\n");
+    printf("U - Tests i2c...\n");
     printf("V - APDS_9960\n");
-    printf("W - Com i2c Pico Annexe\n");
-    printf("X - Com i2c Pico Annexe - non bloquant\n");
-    printf("Y - I2C - Turbine & porte\n");
-    printf("Z - I2C - Turbine & porte + contacteurs - fonctions encapsulees\n");
     stdio_flush();
     int rep = getchar_timeout_us(TEST_TIMEOUT_US);
     stdio_flush();
@@ -182,6 +177,16 @@ int mode_test(){
         while(test_transition_gyro_pas_gyro());
         break;
     
+    case 'R':
+    case 'r':
+        while(test_log());
+        break;
+    
+    case 'S':
+    case 's':
+        while(test_gyro());
+        break;
+
     case 'T':
     case 't':
         while(test_trajectoire());
@@ -189,7 +194,7 @@ int mode_test(){
 
     case 'U':
     case 'u':
-        while(test_i2c_bus());
+        while(test_i2c());
         break;
 
     case 'V':
@@ -197,26 +202,6 @@ int mode_test(){
         while(test_APDS9960());
         break;
 
-    case 'W':
-    case 'w':
-        while(test_i2c_lecture_pico_annex());
-        break;
-
-    case 'X':
-    case 'x':
-        while(test_i2c_lecture_pico_annex_nb2());
-        break;
-
-    case 'Y':
-    case 'y':
-        while(test_i2c_ecriture_pico_annex_nb());
-        break;
-
-    case 'Z':
-    case 'z':
-        while(test_i2c_ecriture_pico_annex_nb_2());
-        break;
-
     case PICO_ERROR_TIMEOUT:
         iteration--;        
         if(iteration == 0){
@@ -270,9 +255,7 @@ int test_capteurs_balise(void){
 
 }
 
-bool reserved_addr(uint8_t addr) {
-    return (addr & 0x78) == 0 || (addr & 0x78) == 0x78;
-}
+
 
 int test_APDS9960(){
     int lettre;
@@ -293,453 +276,6 @@ int test_APDS9960(){
     return 1;
 }
 
-int test_i2c_lecture_pico_annex(){
-    i2c_maitre_init();
-    uint8_t tampon[10];
-    uint8_t registre=2;
-    uint8_t adresse = 0x17;
-    int ret; 
-    
-    ret = i2c_write_blocking(i2c0, adresse,&registre, 1, false);
-    if(ret < 0){
-        printf("Erreur I2C : %d", ret);
-        return 0;
-    }
-
-    ret = i2c_read_blocking(i2c_default, adresse, tampon, 10, false);
-    if(ret < 0){
-        printf("Erreur I2C : %d", ret);
-    }else{
-        for(int i=0; i<10; i++){
-            printf("%c", tampon[i]);
-        }
-        printf("\n");
-
-        for(int i=0; i<10; i++){
-            printf("%2x ", tampon[i]);
-        }
-        printf("\n");
-    }
-    return test_continue_test();
-}
-
-int test_i2c_lecture_pico_annex_nb(){
-    i2c_maitre_init();
-    uint8_t tampon[10];
-    uint8_t registre=2;
-    uint8_t adresse = 0x17;
-    uint32_t time_i2c[5];
-    const uint8_t T_MAX_I2C = 10;
-    int ret; 
-
-    time_i2c[0] = time_us_32();
-
-    // On charge l'adresse de l'esclave
-    i2c0->hw->enable = 0;
-    i2c0->hw->tar = adresse;
-    i2c0->hw->enable = 1;
-
-    // On envoie l'adresse du registre à lire
-    // Pas de stop, pas de restart, écriture : 0, 
-
-    i2c0->hw->data_cmd = registre;
-
-    uint8_t first = false;
-    uint8_t last = false;
-
-    for(int i=0; i<T_MAX_I2C; i++){
-        first = false;
-        last = false;
-        if (i == 0){
-            first = true;
-        }
-        if(i == T_MAX_I2C -1){
-            last = true;
-        }
-
-        i2c0->hw->data_cmd =
-                bool_to_bit(first) << I2C_IC_DATA_CMD_RESTART_LSB |
-                bool_to_bit(last) << I2C_IC_DATA_CMD_STOP_LSB |
-                I2C_IC_DATA_CMD_CMD_BITS; // -> 1 for read
-    }
-
-    time_i2c[1] = time_us_32() - time_i2c[0] ;
-
-    // On attend la fin de la transaction i2c
-    while(i2c0->hw->status & I2C_IC_STATUS_MST_ACTIVITY_BITS);
-
-    time_i2c[2] = time_us_32() - time_i2c[0] ;
-
-    // On lit le tampon I2C
-    // uint8_t * dst;
-    // dst = tampon;
-    
-    for(int i=0; i<T_MAX_I2C; i++){
-        // On attend une donnée
-        while(!i2c_get_read_available(i2c0));
-
-        // Code erreur
-        if(i2c0->hw->tx_abrt_source){
-            printf("Erreur I2C: Abort : %4x\n", i2c0->hw->tx_abrt_source);
-        }
-
-        //On lit la donnée
-        tampon[i] = (uint8_t) i2c0->hw->data_cmd;
-    }
-
-    time_i2c[3] = time_us_32() - time_i2c[0] ;
-
-    // Affichage
-    for(int i=0; i<T_MAX_I2C; i++){
-        printf("%c", tampon[i]);
-    }
-    printf("\n");
-
-    for(int i=0; i<T_MAX_I2C; i++){
-        printf("%2x ", tampon[i]);
-    }
-    printf("\n");
-
-    printf("T_init: %u, T_attente: %u, T_lecture: %u\n", time_i2c[1], time_i2c[2], time_i2c[3]);
-
-    return test_continue_test();
-}
-
-int test_i2c_lecture_pico_annex_nb2(){
-    i2c_maitre_init();
-
-    uint8_t tampon[10];
-    uint8_t registre=8;
-    uint8_t adresse = 0x17;
-    uint32_t time_i2c[5];
-    const uint8_t T_MAX_I2C = 10;
-    enum i2c_resultat_t retour_i2c = I2C_EN_COURS;
-
-    time_i2c[0] = time_us_32();
-    time_i2c[2] = 0;
-
-    while(retour_i2c == I2C_EN_COURS){
-        time_i2c[1] = time_us_32(); // Pour mesurer le temps d'execution
-        i2c_gestion(i2c0);
-        retour_i2c = i2c_lire_registre_nb(adresse, registre, tampon, T_MAX_I2C);
-        time_i2c[2] += time_us_32() - time_i2c[1]; // Pour mesurer le temps d'execution
-        sleep_us(100); // Attente, ou le reste du code
-    }
-    time_i2c[3] = time_us_32() - time_i2c[0];
-
-    // Affichage
-    for(int i=0; i<T_MAX_I2C; i++){
-        printf("%c", tampon[i]);
-    }
-    printf("\n");
-
-    for(int i=0; i<T_MAX_I2C; i++){
-        printf("%2x ", tampon[i]);
-    }
-    printf("\n");
-
-    printf("Temps lecture : %u microsecondes, temps specifique i2c : %u microsecondes.\n", time_i2c[3], time_i2c[2]);
-
-    return test_continue_test();
-}
-
-
-int test_i2c_ecriture_pico_annex_nb(){
-    i2c_maitre_init();
-
-    uint8_t tampon[10];
-    uint8_t registre=0x09;
-    uint8_t adresse = 0x17;
-    uint32_t time_i2c[5];
-    const uint8_t T_I2C_ENVOI = 2;
-    static uint8_t commande=0;
-    enum i2c_resultat_t retour_i2c = I2C_EN_COURS;
-
-
-    printf("F - Ferme porte\n");
-    printf("O - Ouvre porte\n");
-    printf("T - Turbine On\n");
-    printf("U - Turbine Off\n");
-    printf("P - Propulseur On\n");
-    printf("M - Propulseur Off\n");
-    printf("Q pour quitter\n");
-
-    int lettre;
-    do{ 
-        lettre = getchar_timeout_us(0);
-        stdio_flush();
-
-    }while(lettre == PICO_ERROR_TIMEOUT || lettre == '\0');
-
-    tampon[1] = 0x0;
-    switch(lettre){
-        case 'F':
-        case 'f':
-            commande = commande | 0x02; // 0b0000 0010
-            printf("=> Ferme porte\n");
-            break;
-
-        case 'O':
-        case 'o':
-            commande = commande & 0xFD; // 0b1111 1101
-            printf("=> Ouvre porte\n");
-            break;
-        
-        case 't':
-        case 'T':
-            commande = commande | 0x01; // 0b0000 0001
-            printf("=> Active turbine\n");
-            break;
-        
-        case 'u':
-        case 'U':
-            commande = commande & 0xFE; // 0b1111 1110
-            printf("=> Arrete turbine\n");
-            break;
-        
-        case 'p':
-        case 'P':
-            commande = commande | 0x04; // 0b0000 0100
-            printf("=> Active propulseur\n");
-            break;
-        
-        case 'm':
-        case 'M':
-            commande = commande & 0xFB; // 0b1111 1011
-            printf("=> Arrete propulseur\n");
-            break;
-            
-        case 'q':
-        case 'Q':
-            return 0;
-            break;
-
-    }
-
-    tampon[0] = 54;
-    tampon[1] = commande;
-
-    time_i2c[0] = time_us_32();
-    time_i2c[2] = 0;
-
-    while(retour_i2c == I2C_EN_COURS){
-        time_i2c[1] = time_us_32(); // Pour mesurer le temps d'execution
-        i2c_gestion(i2c0);
-        retour_i2c = i2c_ecrire_registre_nb(adresse, registre, tampon, T_I2C_ENVOI);
-        time_i2c[2] += time_us_32() - time_i2c[1]; // Pour mesurer le temps d'execution
-        sleep_us(100); // Attente, ou le reste du code
-    }
-    time_i2c[3] = time_us_32() - time_i2c[0];
-
-    printf("Temps lecture : %u microsecondes, temps specifique i2c : %u microsecondes.\n", time_i2c[3], time_i2c[2]);
-
-    return 1;
-}
-
-void affiche_contacteur(){
-    while(1){
-        printf(">contacteur_butee_A:%d\n", i2c_annexe_get_contacteur_butee_A());
-        printf(">contacteur_butee_C:%d\n", i2c_annexe_get_contacteur_butee_C());
-        printf(">contacteur_longer_A:%d\n", i2c_annexe_get_contacteur_longer_A());
-        printf(">contacteur_longer_C:%d\n", i2c_annexe_get_contacteur_longer_C());
-    }
-}
-
-/// @brief Test les fonctions définies dans I2C_Annexe
-/// @return 1 pour continuer le test, 0 pour arrêter le test
-int test_i2c_ecriture_pico_annex_nb_2(){
-    i2c_maitre_init();
-
-    uint32_t time_i2c[5];
-    const uint8_t T_I2C_ENVOI = 2;
-    static uint8_t commande=0;
-    enum i2c_resultat_t retour_i2c = I2C_EN_COURS;
-
-
-    
-    printf("D - Deguisement On\n");
-    printf("E - Deguisement Off\n");
-    printf("F - Ferme porte\n");
-    printf("G - Mi-Ferme porte\n");
-    printf("O - Ouvre porte\n");
-    printf("T - Turbine On\n");
-    printf("U - Turbine Off\n");
-    printf("P - Propulseur On\n");
-    printf("M - Propulseur Off\n");
-    printf("S - Score + 1\n");
-    printf("B - Bras deplie\n");
-    printf("N - Bras plie 1\n");
-    
-
-    int lettre;
-    int continue_test=1;
-    uint8_t score=0;
-
-    time_i2c[0] = time_us_32();
-    time_i2c[1] = time_us_32();
-    time_i2c[2] = 0;
-
-    multicore_launch_core1(affiche_contacteur);
-
-    while(continue_test){
-        lettre = getchar_timeout_us(0);
-        if(lettre != PICO_ERROR_TIMEOUT && lettre != '\0'){
-            printf("lettre !\n");
-            switch(lettre){
-                case 'd':
-                case 'D':
-                    i2c_annexe_active_deguisement();
-                    printf("=> Active déguisement\n");
-                    break;
-
-                case 'E':
-                case 'e':
-                    i2c_annexe_desactive_deguisement();
-                    printf("=> Desactive déguisement\n");
-                    break;
-
-                case 'F':
-                case 'f':
-                    i2c_annexe_ferme_porte();
-                    printf("=> Ferme porte\n");
-                    break;
-
-                case 'G':
-                case 'g':
-                    i2c_annexe_mi_ferme_porte();
-                    printf("=> Ferme porte\n");
-                    break;
-
-                case 'O':
-                case 'o':
-                    i2c_annexe_ouvre_porte();
-                    printf("=> Ouvre porte\n");
-                    break;
-                
-                case 't':
-                case 'T':
-                    i2c_annexe_active_turbine();
-                    printf("=> Active turbine\n");
-                    break;
-                
-                case 'u':
-                case 'U':
-                    i2c_annexe_desactive_turbine();
-                    printf("=> Arrete turbine\n");
-                    break;
-
-                case 'm':
-                case 'M':
-                    i2c_annexe_desactive_propulseur();
-                    printf("=> Arrete propulseur\n");
-                    break;
-
-                case 'p':
-                case 'P':
-                    i2c_annexe_active_propulseur();
-                    printf("=> Active propulseur\n");
-                    break;
-
-                case 'q':
-                case 'Q':
-                    continue_test=0;
-                    printf("Quitte\n");
-                    break;
-
-                case 's':
-                case 'S':
-                    score++;
-                    i2c_annexe_envoie_score(score);
-                    break;
-
-                case 'b':
-                case 'B':
-                    i2c_annexe_deplie_bras();
-                    printf("=> Deplie bras\n");
-                    break;
-
-                case 'n':
-                case 'N':
-                    i2c_annexe_plie_bras();
-                    printf("=> Plie bras\n");
-                    break;
-                
-                default:
-                    printf("lettre non reconnue: %d %c\n", lettre, lettre);
-            }
-        }
-
-        i2c_gestion(i2c0);        
-        i2c_annexe_gestion();
-
-    }
-    multicore_reset_core1();
-
-    return test_continue_test();
-}
-
-int test_i2c_bus(){
-    // Adresse I2C : 0b0100 000 R/W
-    // Lecture des broches sur les registres 0 et 1
-    // Registre 2 et 3 : valeur des broches en sorties
-    // Registre 4 et 5 : INversion de polarité
-    // Registre 6 et 7 : Configuration entrée (1) ou sortie (0)
-
-    uint8_t reception[8];
-    uint8_t emission[8];
-    //uint8_t adresse = 0b0100000;
-    uint8_t adresse = 0x20;
-    int statu;
-    int lettre;
-
-    emission[0]=6; // Registre à lire
-
-    i2c_maitre_init();
-    // Scan bus I2C - cf SDK
-    printf("\nI2C Bus Scan\n");
-    printf("   0 1 2 3 4 5 6 7 8 9 A B C D E F\n");
-    for (int addr = 0; addr < (1 << 7); ++addr) {
-        if (addr % 16 == 0) {
-            printf("%02x ", addr);
-        }
-        int ret;
-        uint8_t rxdata=0x55;
-        if (reserved_addr(addr))
-            ret = PICO_ERROR_GENERIC;
-        else
-            ret = i2c_read_blocking(i2c_default, addr, &rxdata, 1, false);
-        
-        printf(ret < 0 ? "." : "@");
-        printf(addr % 16 == 15 ? "\n" : " ");
-    }
-    printf("Done.\n");
-    return 0;
-
-    do{    
-        statu = i2c_write_blocking (i2c0, adresse, emission, 1, 0);
-        if(statu == PICO_ERROR_GENERIC){
-            printf("Emission : Address not acknowledged, no device present.\n");
-            return 0;
-        }else{
-            printf("Emission : Ok\n");
-        }
-
-        statu = i2c_read_blocking(i2c0, adresse, reception, 2, 0);
-        if(statu == PICO_ERROR_GENERIC){
-            printf("Reception : Address not acknowledged, no device present.\n");
-            return 0;
-        }else{
-            printf("Recetion : Ok\n");
-        }
-        printf("%2.x%2.x\n",reception[0], reception[1]);
-        
-        lettre = getchar_timeout_us(0);
-        stdio_flush();
-    }while(lettre == PICO_ERROR_TIMEOUT);
-
-    return 0;
-
-}
 
 void test_trajectoire_printf(){
     struct position_t _position;

Test_gyro.c

@@ -0,0 +1,69 @@
+#include "pico/stdlib.h"
+#include "pico/multicore.h"
+#include "gyro.h"
+#include "gyro_ADXRS453.h"
+#include "Log.h"
+#include <stdio.h>
+
+
+#define TEST_TIMEOUT_US 10000000
+
+int test_gyro_vitesse_brute(void);
+
+// Test du gyroscope
+// 1 - lecture continue de la valeur brute de l'angle
+
+int test_gyro(){
+    int lettre;
+
+    while(1){
+
+        do{
+            printf("A : Lecture brute\n");
+            printf("Q : Quitter\n");
+            
+            lettre = getchar_timeout_us(TEST_TIMEOUT_US);
+            stdio_flush();
+        }while(lettre == PICO_ERROR_TIMEOUT ||lettre == 0);
+
+        switch(lettre){
+
+            case 'A':
+            case 'a':
+                while(test_gyro_vitesse_brute());
+                break;
+
+            case 'Q':
+            case 'q':
+                return 0;
+        }
+    }
+}
+
+void affichage(){
+    while(1){
+        Log_gestion();
+    }
+}
+
+int test_gyro_vitesse_brute(void){
+    printf("Lecture vitesse brute\n");
+    uint16_t tampon_envoi[4];
+    uint8_t tampon_reception[4];
+    char message[LOG_MAX_MESSAGE_SIZE];
+    Log_init();
+    Gyro_init_spi();
+    Gyro_init_config();
+
+    struct t_angle_gyro angle_gyro;
+    multicore_launch_core1(affichage);
+    printf("Debut acquisition\n");
+    while(1){
+        gyro_get_vitesse_brute(&angle_gyro, NULL);
+        if(time_us_32() % 100000){
+            sprintf(message,">angle:%d\n", angle_gyro.rot_z);
+            Log_message(message, TELEPLOT);
+        }
+    }
+
+}

Test_gyro.h

@@ -0,0 +1 @@
+int test_gyro(void);

Test_i2c.c

@@ -0,0 +1,421 @@
+#include "pico/stdlib.h"
+#include "pico/multicore.h"
+#include "hardware/i2c.h"
+#include "i2c_annexe.h"
+#include "i2c_maitre.h"
+#include "Test_i2c.h"
+#include <stdio.h>
+
+#define TEST_TIMEOUT_US 10000000
+
+void affiche_contacteur(void);
+
+int test_i2c_bus(void);
+int test_i2c_lecture_pico_annex(void);
+int test_i2c_lecture_pico_annex_nb2(void);
+int test_i2c_ecriture_pico_annex_nb(void);
+int test_i2c_ecriture_pico_annex_nb_2(void);
+bool reserved_addr(uint8_t addr);
+
+
+int test_i2c(){
+    int lettre;
+
+    while(1){
+
+        do{
+            printf("A : Scan bus I2C\n");
+            printf("B : Lecture I2C bloquante\n");
+            printf("C : Lecture I2C non bloquante\n");
+            printf("D : Ecriture I2C non bloquante\n");
+            printf("E : Ecriture I2C non bloquante - fonctions encapsulées\n");            
+            printf("Q : Quitter\n");
+            
+            lettre = getchar_timeout_us(TEST_TIMEOUT_US);
+            stdio_flush();
+        }while(lettre == PICO_ERROR_TIMEOUT ||lettre == 0);
+
+        switch(lettre){
+
+            case 'A':
+            case 'a':
+                while(test_i2c_bus());
+                break;
+
+            case 'B':
+            case 'b':
+                while(test_i2c_lecture_pico_annex());
+                break;
+
+            case 'C':
+            case 'c':
+                while(test_i2c_lecture_pico_annex_nb2());
+                break;
+
+            case 'D':
+            case 'd':
+                while(test_i2c_ecriture_pico_annex_nb());
+                break;
+
+            case 'E':
+            case 'e':
+                while(test_i2c_ecriture_pico_annex_nb_2());
+                break;
+
+            case 'Q':
+            case 'q':
+                return 0;
+        }
+    }
+    
+}
+
+bool reserved_addr(uint8_t addr) {
+    return (addr & 0x78) == 0 || (addr & 0x78) == 0x78;
+}
+
+/// @brief Scan le bus I2C
+/// @return 0
+int test_i2c_bus(){
+    uint8_t reception[8];
+    uint8_t emission[8];
+    //uint8_t adresse = 0b0100000;
+    int statu;
+    int lettre;
+
+    emission[0]=6; // Registre à lire
+
+    i2c_maitre_init();
+    // Scan bus I2C - cf SDK
+    printf("\nI2C Bus Scan\n");
+    printf("   0 1 2 3 4 5 6 7 8 9 A B C D E F\n");
+    for (int addr = 0; addr < (1 << 7); ++addr) {
+        if (addr % 16 == 0) {
+            printf("%02x ", addr);
+        }
+        int ret;
+        uint8_t rxdata=0x55;
+        if (reserved_addr(addr))
+            ret = PICO_ERROR_GENERIC;
+        else{
+            absolute_time_t time_out = get_absolute_time();
+            time_out += 100000; // Ajout 100 ms
+            ret = i2c_read_blocking_until(i2c_default, addr, &rxdata, 1, false, time_out);
+        }
+        
+        printf(ret < 0 ? "." : "@");
+        printf(addr % 16 == 15 ? "\n" : " ");
+    }
+    printf("Done.\n");
+    return 0;
+
+}
+
+
+/// @brief Test de lecture I2C - Attention cette fonction écrit 1 octet avant de lire... 
+/// @return 0
+int test_i2c_lecture_pico_annex(){
+    i2c_maitre_init();
+    uint8_t tampon[10];
+    uint8_t registre=2;
+    uint8_t adresse = 0x17;
+    int ret; 
+    
+    ret = i2c_write_blocking(i2c0, adresse, &registre, 1, false);
+    if(ret < 0){
+        printf("Erreur I2C : %d", ret);
+        return 0;
+    }
+
+    ret = i2c_read_blocking(i2c_default, adresse, tampon, 10, false);
+    if(ret < 0){
+        printf("Erreur I2C : %d", ret);
+    }else{
+        for(int i=0; i<10; i++){
+            printf("%c", tampon[i]);
+        }
+        printf("\n");
+
+        for(int i=0; i<10; i++){
+            printf("%2x ", tampon[i]);
+        }
+        printf("\n");
+    }
+    return 0;
+}
+
+/// @brief Lecture I2C non bloquante
+/// @return 0
+int test_i2c_lecture_pico_annex_nb2(){
+    i2c_maitre_init();
+
+    uint8_t tampon[10];
+    uint8_t registre=8;
+    uint8_t adresse = 0x17;
+    uint32_t time_i2c[5];
+    const uint8_t T_MAX_I2C = 10;
+    enum i2c_resultat_t retour_i2c = I2C_EN_COURS;
+
+    time_i2c[0] = time_us_32();
+    time_i2c[2] = 0;
+
+    while(retour_i2c == I2C_EN_COURS){
+        time_i2c[1] = time_us_32(); // Pour mesurer le temps d'execution
+        i2c_gestion(i2c0);
+        retour_i2c = i2c_lire_registre_nb(adresse, registre, tampon, T_MAX_I2C);
+        time_i2c[2] += time_us_32() - time_i2c[1]; // Pour mesurer le temps d'execution
+        sleep_us(100); // Attente, ou le reste du code
+    }
+    time_i2c[3] = time_us_32() - time_i2c[0];
+
+    // Affichage
+    for(int i=0; i<T_MAX_I2C; i++){
+        printf("%c", tampon[i]);
+    }
+    printf("\n");
+
+    for(int i=0; i<T_MAX_I2C; i++){
+        printf("%2x ", tampon[i]);
+    }
+    printf("\n");
+
+    printf("Temps lecture : %u microsecondes, temps specifique i2c : %u microsecondes.\n", time_i2c[3], time_i2c[2]);
+
+    return 0;
+}
+
+/// @brief Ecrit sur le bus I2C 
+/// @return 1 pour continuer le test, 0 pour quitter
+int test_i2c_ecriture_pico_annex_nb(){
+    i2c_maitre_init();
+
+    uint8_t tampon[10];
+    uint8_t registre=0x09;
+    uint8_t adresse = 0x17;
+    uint32_t time_i2c[5];
+    const uint8_t T_I2C_ENVOI = 2;
+    static uint8_t commande=0;
+    enum i2c_resultat_t retour_i2c = I2C_EN_COURS;
+
+
+    printf("F - Ferme porte\n");
+    printf("O - Ouvre porte\n");
+    printf("T - Turbine On\n");
+    printf("U - Turbine Off\n");
+    printf("P - Propulseur On\n");
+    printf("M - Propulseur Off\n");
+    printf("Q pour quitter\n");
+
+    int lettre;
+    do{ 
+        lettre = getchar_timeout_us(0);
+        stdio_flush();
+
+    }while(lettre == PICO_ERROR_TIMEOUT || lettre == '\0');
+
+    tampon[1] = 0x0;
+    switch(lettre){
+        case 'F':
+        case 'f':
+            commande = commande | 0x02; // 0b0000 0010
+            printf("=> Ferme porte\n");
+            break;
+
+        case 'O':
+        case 'o':
+            commande = commande & 0xFD; // 0b1111 1101
+            printf("=> Ouvre porte\n");
+            break;
+        
+        case 't':
+        case 'T':
+            commande = commande | 0x01; // 0b0000 0001
+            printf("=> Active turbine\n");
+            break;
+        
+        case 'u':
+        case 'U':
+            commande = commande & 0xFE; // 0b1111 1110
+            printf("=> Arrete turbine\n");
+            break;
+        
+        case 'p':
+        case 'P':
+            commande = commande | 0x08; // 0b0000 1000
+            printf("=> Active propulseur\n");
+            break;
+        
+        case 'm':
+        case 'M':
+            commande = commande & 0xF7; // 0b1111 0111
+            printf("=> Arrete propulseur\n");
+            break;
+            
+        case 'q':
+        case 'Q':
+            return 0;
+            break;
+
+    }
+
+    tampon[0] = 54;
+    tampon[1] = commande;
+
+    time_i2c[0] = time_us_32();
+    time_i2c[2] = 0;
+
+    while(retour_i2c == I2C_EN_COURS){
+        time_i2c[1] = time_us_32(); // Pour mesurer le temps d'execution
+        i2c_gestion(i2c0);
+        retour_i2c = i2c_ecrire_registre_nb(adresse, registre, tampon, T_I2C_ENVOI);
+        time_i2c[2] += time_us_32() - time_i2c[1]; // Pour mesurer le temps d'execution
+        sleep_us(100); // Attente, ou le reste du code
+    }
+    time_i2c[3] = time_us_32() - time_i2c[0];
+
+    printf("Temps lecture : %u microsecondes, temps specifique i2c : %u microsecondes.\n", time_i2c[3], time_i2c[2]);
+
+    return 1;
+}
+
+
+/// @brief Test les fonctions définies dans I2C_Annexe
+/// @return 0
+int test_i2c_ecriture_pico_annex_nb_2(){
+    i2c_maitre_init();
+
+    uint32_t time_i2c[5];
+    const uint8_t T_I2C_ENVOI = 2;
+    static uint8_t commande=0;
+    enum i2c_resultat_t retour_i2c = I2C_EN_COURS;
+
+
+    
+    printf("D - Deguisement On\n");
+    printf("E - Deguisement Off\n");
+    printf("F - Ferme porte\n");
+    printf("G - Mi-Ferme porte\n");
+    printf("O - Ouvre porte\n");
+    printf("T - Turbine On\n");
+    printf("U - Turbine Off\n");
+    printf("P - Propulseur On\n");
+    printf("M - Propulseur Off\n");
+    printf("S - Score + 1\n");
+    printf("B - Bras deplie\n");
+    printf("N - Bras plie 1\n");
+    
+
+    int lettre;
+    int continue_test=1;
+    uint8_t score=0;
+
+    time_i2c[0] = time_us_32();
+    time_i2c[1] = time_us_32();
+    time_i2c[2] = 0;
+
+    multicore_launch_core1(affiche_contacteur);
+
+
+    while(1){
+        lettre = getchar_timeout_us(0);
+        if(lettre != PICO_ERROR_TIMEOUT && lettre != '\0'){
+            printf("lettre !\n");
+            switch(lettre){
+                case 'd':
+                case 'D':
+                    i2c_annexe_active_deguisement();
+                    printf("=> Active déguisement\n");
+                    break;
+
+                case 'E':
+                case 'e':
+                    i2c_annexe_desactive_deguisement();
+                    printf("=> Desactive déguisement\n");
+                    break;
+
+                case 'F':
+                case 'f':
+                    i2c_annexe_ferme_porte();
+                    printf("=> Ferme porte\n");
+                    break;
+
+                case 'G':
+                case 'g':
+                    i2c_annexe_mi_ferme_porte();
+                    printf("=> Ferme porte\n");
+                    break;
+
+                case 'O':
+                case 'o':
+                    i2c_annexe_ouvre_porte();
+                    printf("=> Ouvre porte\n");
+                    break;
+                
+                case 't':
+                case 'T':
+                    i2c_annexe_active_turbine();
+                    printf("=> Active turbine\n");
+                    break;
+                
+                case 'u':
+                case 'U':
+                    i2c_annexe_desactive_turbine();
+                    printf("=> Arrete turbine\n");
+                    break;
+
+                case 'm':
+                case 'M':
+                    i2c_annexe_desactive_propulseur();
+                    printf("=> Arrete propulseur\n");
+                    break;
+
+                case 'p':
+                case 'P':
+                    i2c_annexe_active_propulseur();
+                    printf("=> Active propulseur\n");
+                    break;
+
+                case 'q':
+                case 'Q':
+                    multicore_reset_core1();
+                    return 0;
+
+                case 's':
+                case 'S':
+                    score++;
+                    i2c_annexe_envoie_score(score);
+                    break;
+
+                case 'b':
+                case 'B':
+                    i2c_annexe_deplie_bras();
+                    printf("=> Deplie bras\n");
+                    break;
+
+                case 'n':
+                case 'N':
+                    i2c_annexe_plie_bras();
+                    printf("=> Plie bras\n");
+                    break;
+                
+                default:
+                    printf("lettre non reconnue: %d %c\n", lettre, lettre);
+            }
+        }
+
+        i2c_gestion(i2c0);        
+        i2c_annexe_gestion();
+
+    }
+    
+
+}
+
+void affiche_contacteur(){
+    while(1){
+        printf(">contacteur_butee_A:%d\n", i2c_annexe_get_contacteur_butee_A());
+        printf(">contacteur_butee_C:%d\n", i2c_annexe_get_contacteur_butee_C());
+        printf(">contacteur_longer_A:%d\n", i2c_annexe_get_contacteur_longer_A());
+        printf(">contacteur_longer_C:%d\n", i2c_annexe_get_contacteur_longer_C());
+    }
+}

Test_i2c.h

@@ -0,0 +1,2 @@
+
+int test_i2c(void);

Test_log.c

@@ -0,0 +1,80 @@
+#include "Log.h"
+#include "pico/multicore.h"
+#include <stdio.h>
+
+#define TEST_TIMEOUT_US 10000000
+
+void log_core1_routine(){
+    while(1){
+        Log_gestion();
+    }
+}
+
+
+/// @brief Fonction pour testes les fonctions Log
+/// @param  
+/// @return 1 si la doit être ré-appelée, 0 si l'utilisateur quitte.
+int test_log(void){
+    int lettre;
+    char message[50];
+
+    Log_init();
+
+    multicore_launch_core1(log_core1_routine);
+
+    while(1){
+
+        do{
+            printf("A : 1 message\n");
+            printf("B : 9 messages d'affilés\n");
+            printf("C : 100 messages à 100 µs d'intervalle\n");
+            printf("D : Récupération du log stocké\n");
+            printf("Q : Quitter\n");
+            lettre = getchar_timeout_us(TEST_TIMEOUT_US);
+            stdio_flush();
+        }while(lettre == PICO_ERROR_TIMEOUT ||lettre == 0);
+
+        
+
+        switch(lettre){
+            case 'A':
+            case 'a':
+                printf("A : 1 message\n");
+                Log_message("Test 1 message", DEBUG);
+                break;
+
+            case 'B':
+            case 'b':
+                printf("B : 9 messages d'affilés\n");
+                for(int i=0; i<9; i++){
+                    sprintf(message, "Test message %d/9", i+1);
+                    Log_message(message, DEBUG);
+                }
+                break;
+
+            case 'C':
+            case 'c':
+                printf("C : 100 messages à 100 µs d'intervalle\n");
+                for(int i=0; i<100; i++){
+                    sprintf(message, "Test message %d/100", i+1);
+                    Log_message(message, DEBUG);
+                    sleep_us(100);
+                }
+                break;
+
+            case 'D':
+            case 'd':
+                printf("D : Récupération du log stocké\n");
+                Log_get_full_log();
+                break;
+
+            case 'Q':
+            case 'q':
+                printf("Q : Quitter\n");
+                multicore_reset_core1();
+                return 0;
+                break;
+        }
+    }
+}
+

Test_log.h

@@ -0,0 +1 @@
+int test_log(void);

Trajet.h

@@ -10,6 +10,8 @@ enum etat_trajet_t{
 #define TRAJECT_CONFIG_AVANCE_DROIT 1000, 500
 // Vitesse et acceleration pour un mouvement complexe (en mm et mm/s²)
 #define TRAJECT_CONFIG_AVANCE_ET_TOURNE 300, 500
+// Vitesse et acceleration - standard (en mm et mm/s²)
+#define TRAJECT_CONFIG_STD 500, 500
 // Vitesse et acceleration pour une rotation (rad/s et rad/s²)
 #define TRAJECT_CONFIG_ROTATION_PURE 2, 2
 

gyro.c

@@ -50,8 +50,7 @@ struct t_angle_gyro_float gyro_get_vitesse(void){
     return vitesse_gyro;
 }
 
-void Gyro_Init(void){
-    // 
+void Gyro_init_spi(){
     gpio_set_function(0, GPIO_FUNC_SPI); // SDI (ancien : 16)
     gpio_set_function(2, GPIO_FUNC_SPI); // SCK (ancien : 18)
     gpio_set_function(3, GPIO_FUNC_SPI); // SDO (ancien : 19)
@@ -64,19 +63,20 @@ void Gyro_Init(void){
     vitesse_calibration = NULL;
     vitesse_angulaire = &_vitesse_angulaire;
 
-    //uint speed = spi_init(spi0, 10 * 1000); // SPI init @ 10 kHz
     uint speed = spi_init(spi0, 2 * 1000 * 1000); // SPI init @ 2 MHz    
-    
-    //printf("vitesse SPI : %d\n", speed);
-
+    // Normalement, le capteur tient jusqu'à 8 MHz, à voir si c'est pertinent
+    //uint speed = spi_init(spi0, 8 * 1000 * 1000); // SPI init @ 8 MHz    
 
     spi_set_format(spi0, 8, SPI_CPHA_0, SPI_CPOL_0, SPI_MSB_FIRST);
+}
 
+void Gyro_init_config(){
     // Test de la présence du gyroscope :
     if(gyro_init_check()){
         Monitoring_Error("Gyroscope non trouve");
         while(1){
             puts("Gyroscope non trouve");
+            sleep_ms(500);
         }; // On s'arrête là !
     }else{
         //puts("Gyroscope trouve");
@@ -86,12 +86,23 @@ void Gyro_Init(void){
             Monitoring_Error("gyro_config FAILED !");
             while(1){
                 puts("gyro_config FAILED !");
+                sleep_ms(500);
             }; // On s'arrête là !
 
         }
     }
     sleep_ms(150); // Temps d'init du gyroscope
+}
 
+
+void Gyro_Init(void){
+    // Initialisation du SPI
+    Gyro_init_spi();
+    
+    // Initialisation fonctionnelle du gyroscope
+    Gyro_init_config();
+    
+    // Calibration du gyroscope
     gyro_calibration();
     
 }

gyro.h

@@ -7,3 +7,5 @@ void gyro_set_angle_radian(float angle_radian);
 struct t_angle_gyro_float gyro_get_angle_degres(void);
 struct t_angle_gyro_float gyro_get_vitesse(void);
 int16_t gyro_get_temp(void);
+void Gyro_init_spi(void);
+void Gyro_init_config(void);

gyro_ADXRS453.c

@@ -49,7 +49,10 @@ void affiche_tampon_32bits(uint8_t *tampon){
 
 }
 
-
+/// @brief Lit les données du gyroscope
+/// @param tampon_envoi espace mémoire à fournir à la fonction 
+/// @param tampon_reception espace mémoire à fournir à la fonction 
+/// @return 1 en cas d'erreur, 0 sinon
 int gyro_get_sensor_data(uint16_t tampon_envoi[], uint8_t tampon_reception[]){
     tampon_envoi[0] = 0x30;
     tampon_envoi[1] = 0x00;
@@ -61,28 +64,29 @@ int gyro_get_sensor_data(uint16_t tampon_envoi[], uint8_t tampon_reception[]){
         
         Monitoring_Error("Gyro Failed - SQ bits != 0x4\n");
         if(tampon_reception[1] & 0x04){
+            set_position_avec_gyroscope_error(1);
             //printf("SPI ERROR\n");
             return 1;
         }else{
+            set_position_avec_gyroscope_error(1);
             Monitoring_set_erreur_critique();
             //while(1){
                 affiche_tampon_32bits(tampon_reception);
                 printf("Gyro Failed - SQ bits (%#3x)!= 0x4\n", Gyro_SensorData.SQ);
             //}
-            //set_position_avec_gyroscope(0);
         }
     }
     if(Gyro_SensorData.ST != 0x1){
         Monitoring_Error("Gyro Failed - Status != 0x1\n");
-        set_position_avec_gyroscope(0);
+        set_position_avec_gyroscope_error(1);
         /*while(1){
             printf("Gyro Failed - Status (%#3x)!= 0x1\n", Gyro_SensorData.ST);
             affiche_tampon_32bits(tampon_reception);
         }*/
         Monitoring_set_erreur_critique();
-        //set_position_avec_gyroscope(0);
         return 1;
     }
+    set_position_avec_gyroscope_error(0);
     return 0;
 }
 
@@ -175,7 +179,8 @@ void gyro_get_vitesse_brute(struct t_angle_gyro* angle_gyro, struct t_angle_gyro
     uint8_t tampon_reception[5]="\0\0\0\0\0";
     int16_t rot_z;
     
-    sleep_us(1); // A supprimer plus tard
+    // Supprimé le 11/7/2023, pas d'impacts visibles..
+    //sleep_us(1); // A supprimer plus tard
     if(gyro_get_sensor_data(tampon_envoi, tampon_reception)){
         return;
     }

i2c_maitre.c

@@ -44,6 +44,8 @@ void i2c_maitre_init(void){
     printf("%d et %d en I2C\n", I2C_SDA_PIN, I2C_SCL_PIN);
     gpio_set_function(I2C_SDA_PIN, GPIO_FUNC_I2C);
     gpio_set_function(I2C_SCL_PIN, GPIO_FUNC_I2C);
+    gpio_pull_up(I2C_SDA_PIN);
+    gpio_pull_up(I2C_SDA_PIN);
 
     i2c_statu_i2c0 = I2C_STATU_LIBRE;
 }