From 8f5a160a75665217cc3b974fb4f1ff532ba75a03 Mon Sep 17 00:00:00 2001
From: Samuel <samuel.kay@poivron-robotique.fr>
Date: Tue, 22 Aug 2023 23:06:02 +0200
Subject: [PATCH] Tests d'endurance + utilisation des log
---
Localisation.c | 2 +-
Log.c | 15 ++++-
Log.h | 5 +-
Monitoring.c | 2 +
Test.c | 179 +++++++++++++++++++++++++++++++++++++++++++++++--
Test_i2c.c | 1 +
Test_log.c | 12 ++++
7 files changed, 208 insertions(+), 8 deletions(-)
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 <stdio.h>
#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");