From f7bebda9d24f366fa4680cbee9ecbf443378ebe9 Mon Sep 17 00:00:00 2001 From: Samuel Date: Thu, 18 May 2023 14:51:16 +0200 Subject: [PATCH] =?UTF-8?q?Tentative=20cerise=20lat=C3=A9rales=20+=20corre?= =?UTF-8?q?ctions=20erreurs=20Gyro?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Holonome2023.c | 4 +++- Monitoring.c | 9 +++++++-- Strategie.c | 6 ++++-- gyro_ADXRS453.c | 8 ++++++-- 4 files changed, 20 insertions(+), 7 deletions(-) diff --git a/Holonome2023.c b/Holonome2023.c index 2379f8e..72b931e 100644 --- a/Holonome2023.c +++ b/Holonome2023.c @@ -90,12 +90,14 @@ int main() { Trajet_init(); Balise_VL53L1X_init(); + multicore_launch_core1(Monitoring_display); + 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; diff --git a/Monitoring.c b/Monitoring.c index 7d2862f..7d14e90 100644 --- a/Monitoring.c +++ b/Monitoring.c @@ -2,6 +2,7 @@ #include #include "Monitoring.h" #include "Localisation.h" +#include "Trajet.h" #include "Asser_Moteurs.h" #include "i2c_annexe.h" @@ -42,17 +43,21 @@ void Monitoring_display(){ temps_cycle_display(); printf(">V_bat:%ld:%2.1f\n", temps, (float) (i2c_annexe_get_tension_batterie() / 10.)); printf(">DebugVar:%ld:%d\n", temps, debug_var); - printf(">DebugVarf:%ld:%f\n", temps, debug_varf); + printf(">Distance_Obstacle:%ld:%f\n", temps, Trajet_get_obstacle_mm()); + printf(">DebugVarf(abscisse):%ld:%f\n", temps, debug_varf); struct position_t position = Localisation_get(); printf(">pos_x:%ld:%f\n", temps, position.x_mm); + printf(">P_con_x:%ld:%f\n", temps, Trajet_get_consigne().x_mm); printf(">pos_y:%ld:%f\n", temps, position.y_mm); + printf(">P_con_y:%ld:%f\n", temps, Trajet_get_consigne().y_mm); + printf(">pos_angle:%ld:%f\n", temps, position.angle_radian); + printf(">P_con_angle:%ld:%f\n", temps, Trajet_get_consigne().angle_radian); printf(">V_a:%ld:%f\n", temps, AsserMoteur_getVitesse_mm_s(MOTEUR_A, 1)); printf(">V_b:%ld:%f\n", temps, AsserMoteur_getVitesse_mm_s(MOTEUR_B, 1)); printf(">V_c:%ld:%f\n", temps, AsserMoteur_getVitesse_mm_s(MOTEUR_C, 1)); printf(">V_con_a:%ld:%f\n", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_A)); printf(">V_con_b:%ld:%f\n", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_B)); printf(">V_con_c:%ld:%f\n", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_C)); - } } diff --git a/Strategie.c b/Strategie.c index 0d6f333..c9b5110 100644 --- a/Strategie.c +++ b/Strategie.c @@ -87,7 +87,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ 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 = FAIT, .cible = CERISE_GAUCHE}; + struct objectif_t objectif_3 = { .priorite = 2, .etat = A_FAIRE, .cible = CERISE_GAUCHE}; struct objectif_t objectif_4 = { .priorite = 5, .etat = FAIT, .cible = CERISE_DROITE}; objectifs[0]= objectif_1; objectifs[1]= objectif_2; @@ -97,7 +97,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ 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 = FAIT, .cible = CERISE_DROITE}; + struct objectif_t objectif_3 = { .priorite = 2, .etat = A_FAIRE, .cible = CERISE_DROITE}; struct objectif_t objectif_4 = { .priorite = 5, .etat = FAIT, .cible = CERISE_GAUCHE}; objectifs[0]= objectif_1; objectifs[1]= objectif_2; @@ -118,6 +118,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ 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, 500); Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 156, @@ -154,6 +155,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ 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, 500); Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 175, diff --git a/gyro_ADXRS453.c b/gyro_ADXRS453.c index 25c0fca..abca285 100644 --- a/gyro_ADXRS453.c +++ b/gyro_ADXRS453.c @@ -1,5 +1,6 @@ #include "gyro_ADXRS453.h" #include "Monitoring.h" +#include "Robot_config.h" #include "spi_nb.h" #include @@ -57,14 +58,17 @@ int gyro_get_sensor_data(uint16_t tampon_envoi[], uint8_t tampon_reception[]){ gyro_spi_wr_32bits(tampon_envoi, tampon_reception); Gyro_traitementDonnees(tampon_reception); if(Gyro_SensorData.SQ != 0x4){ - //printf("Gyro Failed - SQ bits (%#3x)!= 0x4\n", Gyro_SensorData.SQ); - //affiche_tampon_32bits(tampon_reception); + printf("Gyro Failed - SQ bits (%#3x)!= 0x4\n", Gyro_SensorData.SQ); + affiche_tampon_32bits(tampon_reception); Monitoring_Error("Gyro Failed - SQ bits != 0x4\n"); + set_position_avec_gyroscope(0); return 1; } if(Gyro_SensorData.ST != 0x1){ printf("Gyro Failed - Status (%#3x)!= 0x1\n", Gyro_SensorData.ST); + Monitoring_Error("Gyro Failed - Status != 0x1\n"); affiche_tampon_32bits(tampon_reception); + set_position_avec_gyroscope(0); return 1; } return 0;