diff --git a/Localisation.c b/Localisation.c index dd530dc..f79363d 100644 --- a/Localisation.c +++ b/Localisation.c @@ -55,7 +55,7 @@ void Localisation_gestion(){ position.angle_radian += variation_angle; // On utilise aussi les roue codeuses pour ajuster la position du gyroscope tant qu'il est indisponible - gyro_set_angle_radian(position.angle_radian ) + gyro_set_angle_radian(position.angle_radian); } }else{ diff --git a/Log.c b/Log.c index 9c8ef04..1197551 100644 --- a/Log.c +++ b/Log.c @@ -8,7 +8,7 @@ struct log_message_storage{ struct log_message_storage * next; } * log_message_envoi, log_message_storage_premier, *log_message_storage_courant; -uint log_error = 0; +uint log_error = LOG_ERROR_LOG_NOT_INIT; enum Log_level log_level_reference; int store_new_message(struct Log_message_data message, struct log_message_storage * stockage); @@ -24,6 +24,7 @@ void Log_init(void){ log_message_envoi = &log_message_storage_premier; log_level_reference = TELEPLOT; + log_error = LOG_ERROR_OK; } @@ -31,6 +32,9 @@ void Log_init(void){ /// @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){ + if(log_error != LOG_ERROR_OK){ + return; + } // 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 @@ -59,6 +63,9 @@ void Log_message(char * message, enum Log_level log_level){ /// @brief Read messages in cache, store them and send them through the serial connection void Log_gestion(){ + if(log_error == LOG_ERROR_LOG_NOT_INIT){ + return; + } // Envoi 1 message par la liaison série if(log_message_envoi->next != NULL){ log_message_envoi = log_message_envoi->next; @@ -84,3 +91,9 @@ void envoi_message(struct Log_message_data message_data){ printf("%u ms:%s\n", message_data.timestamp, message_data.message); } } + +/// @brief Renvoi le status du log +/// @return LOG_ERROR_OK ou LOG_ERROR_MEMORY_FULL ou LOG_ERROR_LOG_NOT_INIT +int Log_get_statut(){ + return log_error; +} diff --git a/Log.h b/Log.h index b856705..d699010 100644 --- a/Log.h +++ b/Log.h @@ -2,7 +2,9 @@ #define LOG_MAX_MESSAGE_SIZE 64 +#define LOG_ERROR_OK 0 #define LOG_ERROR_MEMORY_FULL 1 +#define LOG_ERROR_LOG_NOT_INIT 2 enum Log_level{ TELEPLOT, TRACE, DEBUG, INFO, WARN, ERROR, FATAL @@ -17,4 +19,5 @@ struct Log_message_data{ void Log_init(void); void Log_gestion(void); void Log_message(char * message, enum Log_level log_level); -void Log_get_full_log(); \ No newline at end of file +void Log_get_full_log(); +int Log_get_statut(); diff --git a/Monitoring.c b/Monitoring.c index c2d3e40..c09e53c 100644 --- a/Monitoring.c +++ b/Monitoring.c @@ -2,6 +2,7 @@ #include #include "Monitoring.h" #include "Localisation.h" +#include "Log.h" #include "Trajet.h" #include "Asser_Moteurs.h" #include "i2c_annexe.h" @@ -90,6 +91,7 @@ void set_debug_varf(float variable){ void Monitoring_Error(char * msg){ gpio_put(LED_PIN_ROUGE, 1); + Log_message(msg, ERROR); } diff --git a/Test.c b/Test.c index 6c4a437..b9d6670 100644 --- a/Test.c +++ b/Test.c @@ -18,6 +18,7 @@ #include "i2c_annexe.h" #include "i2c_maitre.h" #include "Localisation.h" +#include "Log.h" #include "Moteurs.h" #include "Monitoring.h" #include "QEI.h" @@ -56,11 +57,14 @@ int test_transition_gyro_pas_gyro(void); int test_trajectoire(void); void affiche_localisation(void); int test_aller_retour(); +int test_endurance_aller_retour(); void test_trajectoire_teleplot(); int test_capteurs_balise(void); int test_geometrie(void); int test_angle_balise(void); +int continuous_printf = 1; + // Mode test : renvoie 0 pour quitter le mode test int mode_test(){ @@ -88,6 +92,7 @@ int mode_test(){ printf("T - Trajectoire\n"); printf("U - Tests i2c...\n"); printf("V - APDS_9960\n"); + printf("W - Endurance aller retour\n"); stdio_flush(); int rep = getchar_timeout_us(TEST_TIMEOUT_US); stdio_flush(); @@ -202,6 +207,11 @@ int mode_test(){ while(test_APDS9960()); break; + case 'W': + case 'w': + while(test_endurance_aller_retour()); + break; + case PICO_ERROR_TIMEOUT: iteration--; if(iteration == 0){ @@ -294,11 +304,14 @@ void test_trajectoire_teleplot(){ _position = Localisation_get(); uint32_t temps; temps = time_us_32()/1000; - printf(">X:%ld:%f\n>Y:%ld:%f\n>orientation:%ld:%f\n", temps, _position.x_mm, temps, _position.y_mm, temps, _position.angle_radian/M_PI*180); - printf(">Consigne_X:%ld:%f\n>Consigne_Y:%ld:%f\n>Consigne_orientation:%ld:%f\n", temps, _consigne.x_mm, temps, _consigne.y_mm, temps, _consigne.angle_radian/M_PI*180); - printf(">Position:%f:%f:%ld|xy\n>Consigne_Position:%f:%f:%ld|xy\n", _position.x_mm, _position.y_mm, temps, _consigne.x_mm, _consigne.y_mm, temps); - printf(">V_A:%ld:%f\n>V_B:%ld:%f\n>V_C:%ld:%f\n", temps, QEI_get_mm(QEI_A_NAME), temps, QEI_get_mm(QEI_B_NAME), temps, QEI_get_mm(QEI_C_NAME)); - printf(">V_consigne_A:%ld:%f\n>V_consigne_B:%ld:%f\n>V_consigne_C:%ld:%f\n", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_A), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_B), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_C)); + if(continuous_printf){ + printf(">X:%ld:%f\n>Y:%ld:%f\n>orientation:%ld:%f\n", temps, _position.x_mm, temps, _position.y_mm, temps, _position.angle_radian/M_PI*180); + printf(">Consigne_X:%ld:%f\n>Consigne_Y:%ld:%f\n>Consigne_orientation:%ld:%f\n", temps, _consigne.x_mm, temps, _consigne.y_mm, temps, _consigne.angle_radian/M_PI*180); + printf(">Position:%f:%f:%ld|xy\n>Consigne_Position:%f:%f:%ld|xy\n", _position.x_mm, _position.y_mm, temps, _consigne.x_mm, _consigne.y_mm, temps); + printf(">V_A:%ld:%f\n>V_B:%ld:%f\n>V_C:%ld:%f\n", temps, QEI_get_mm(QEI_A_NAME), temps, QEI_get_mm(QEI_B_NAME), temps, QEI_get_mm(QEI_C_NAME)); + printf(">V_consigne_A:%ld:%f\n>V_consigne_B:%ld:%f\n>V_consigne_C:%ld:%f\n", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_A), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_B), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_C)); + } + Log_gestion(); } } @@ -416,6 +429,162 @@ int test_aller_retour(){ } +/// @brief Fonction de test d'endurance, notamment pour valider le bon fonctionnement du gyroscope +/// sur le long terme et en dynamique. Tente aussi de valider les enregistrements des messages de log. +/// Pour l'instant aucune erreur enregistrée, donc pas hyper probant. +/// @return +int test_endurance_aller_retour(){ + int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2; + const float corr_angle = 1.145; + Trajet_init(); + Log_init(); + struct trajectoire_t trajectoire; + printf("Choix trajectoire :\n"); + printf("B - Bezier\n"); + printf("C - Circulaire\n"); + printf("D - Droite\n"); + printf("E - Avance et tourne (ok)\n"); + printf("F - Avance et tourne (Nok)\n"); + printf("G - Avance (Calibre angle)\n"); + printf("R - Rotation pure\n"); + do{ + lettre = getchar_timeout_us(TEST_TIMEOUT_US); + stdio_flush(); + }while(lettre == PICO_ERROR_TIMEOUT); + switch(lettre){ + case 'b': + case 'B': + Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT); + Trajectoire_bezier(&trajectoire, 0, 0, -200., 450, 250, 450, 0, 0, 0, 0); + Log_message("Trajectoire de Bézier\n", INFO); + break; + + case 'c': + case 'C': + Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE); + Trajectoire_circulaire(&trajectoire, 0, 250, -90, 90, 250, 0, 0); + printf("Trajectoire circulaire\n"); + break; + + case 'd': + case 'D': + Trajectoire_droite(&trajectoire, 0, 0, 0, 700, 0, 0); + Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT); + printf("Trajectoire droite\n"); + break; + + case 'e': + case 'E': + Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE); + Trajectoire_droite(&trajectoire, 0, 0, 0, 700, 0, M_PI); + printf("Trajectoire droite avec rotation (OK)\n"); + break; + + case 'f': + case 'F': + Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT); + Trajectoire_droite(&trajectoire, 0, 0, 0, 700, 0, M_PI); + printf("Trajectoire droite avec rotation (Nok)\n"); + break; + + case 'g': + case 'G': + Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE); + Trajectoire_droite(&trajectoire, 0, 0, + 2750 * cos((60+90-corr_angle) * (M_PI / 180.)), 2750 * sin((60+90-corr_angle) * (M_PI / 180.)), + 0, 0); + printf("Trajectoire droite pour calibration angle de départ\n"); + break; + + case 'r': + case 'R': + Trajectoire_rotation(&trajectoire, 0, 0, 0, 700); + trajectoire.orientation_debut_rad = 0; + trajectoire.orientation_fin_rad = M_PI; + printf("Trajectoire droite avec rotation\n"); + break; + + default: return 0; + } + + printf("Init gyroscope\n"); + Gyro_Init(); + //printf("C'est parti !\n"); + stdio_flush(); + + set_position_avec_gyroscope(1); + + Trajet_debut_trajectoire(trajectoire); + multicore_launch_core1(test_trajectoire_teleplot); + temps_ms = Temps_get_temps_ms(); + int nb_aller=0; + do{ + // Routines à 1 ms + while(temps_ms == Temps_get_temps_ms()); + temps_ms = Temps_get_temps_ms(); + QEI_update(); + Localisation_gestion(); + + // Routine à 2 ms + if(temps_ms % _step_ms_gyro == 0){ + Gyro_Read(_step_ms_gyro); + } + + // Routine à 100 ms + if(temps_ms % 100 == 0){ + char message[256]; + uint32_t temps; + struct position_t _position, _consigne; + temps = time_us_32()/1000; + _consigne = Trajet_get_consigne(); + _position = Localisation_get(); + + /*sprintf(message, ">X:%ld:%.2f\n>Y:%ld:%.2f\n>orientation:%ld:%.2f", temps, _position.x_mm, temps, _position.y_mm, temps, _position.angle_radian/M_PI*180); + Log_message(message, TELEPLOT); + sprintf(message, ">C_X:%ld:%.2f\n>C_Y:%ld:%.2f\n>C_orientation:%ld:%.2f", temps, _consigne.x_mm, temps, _consigne.y_mm, temps, _consigne.angle_radian/M_PI*180); + Log_message(message, TELEPLOT); + sprintf(message, ">V_A:%ld:%.2f\n>V_B:%ld:%.2f\n>V_C:%ld:%.2f", temps, QEI_get_mm(QEI_A_NAME), temps, QEI_get_mm(QEI_B_NAME), temps, QEI_get_mm(QEI_C_NAME)); + Log_message(message, TELEPLOT); + sprintf(message, ">Vc_A:%ld:%.2f\n>Vc_B:%ld:%.2f\n>Vc_C:%ld:%.2f", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_A), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_B), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_C)); + Log_message(message, TELEPLOT);*/ + + } + + if(Trajet_avance(_step_ms/1000.) == TRAJET_TERMINE){ + char message[256]; + nb_aller++; + Trajectoire_inverse(&trajectoire); + Trajet_debut_trajectoire(trajectoire); + if(nb_aller % 2){ + sprintf(message, "Aller %d\n", nb_aller / 2 +1); + }else{ + sprintf(message, "retour %d\n", nb_aller / 2); + } + Log_message(message, INFO); + }else{ + AsserMoteur_Gestion(_step_ms); + } + lettre = getchar_timeout_us(0); + //lettre = PICO_ERROR_TIMEOUT; + }while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0)); + printf("Lettre : %d; %c\n", lettre, lettre); + + Moteur_Stop(); + continuous_printf = 0; + printf("Boucle Log\n"); + do{ + Log_gestion(); + lettre = getchar_timeout_us(0); + if(lettre == 'L'){ + Log_get_full_log(); + } + }while(lettre != 'q'); + printf("Fin boucle Log\n"); + + return 0; + +} + int test_trajectoire(){ diff --git a/Test_i2c.c b/Test_i2c.c index a7a1345..b679965 100644 --- a/Test_i2c.c +++ b/Test_i2c.c @@ -417,5 +417,6 @@ void affiche_contacteur(){ 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()); + printf(">v_bat:%d\n", i2c_annexe_get_tension_batterie()); } } diff --git a/Test_log.c b/Test_log.c index 5bb8922..f9f374c 100644 --- a/Test_log.c +++ b/Test_log.c @@ -29,6 +29,7 @@ int test_log(void){ 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("E - Messages jusqu'à saturation du Log\n"); printf("Q : Quitter\n"); lettre = getchar_timeout_us(TEST_TIMEOUT_US); stdio_flush(); @@ -68,6 +69,17 @@ int test_log(void){ Log_get_full_log(); break; + case 'E': + case 'e': + printf("E - Messages jusqu'à saturation du Log\n"); + int i; + while(Log_get_statut() == LOG_ERROR_OK){ + sprintf(message, "Test message %d", i++); + Log_message(message, DEBUG); + } + Log_get_full_log(); + break; + case 'Q': case 'q': printf("Q : Quitter\n");