From cca330ae89ff5f84e65fe486e66a0f984e2e90e0 Mon Sep 17 00:00:00 2001 From: Samuel Date: Fri, 19 May 2023 01:36:39 +0200 Subject: [PATCH] Ajout de l'erreur critique pour le gyro qui arrete la strategie --- Monitoring.c | 12 +++++++++++- Monitoring.h | 4 +++- Strategie.c | 26 ++++++++++++++------------ Strategie.h | 19 ------------------- Test_strategie.c | 1 - gyro_ADXRS453.c | 6 ++++-- 6 files changed, 32 insertions(+), 36 deletions(-) diff --git a/Monitoring.c b/Monitoring.c index 7d14e90..e6f7797 100644 --- a/Monitoring.c +++ b/Monitoring.c @@ -10,6 +10,7 @@ uint32_t temps_cycle_min = UINT32_MAX; uint32_t temps_cycle_max=0; +int erreur_critique = 0; int lock=0; uint32_t debug_var=0; float debug_varf=0; @@ -88,4 +89,13 @@ void set_debug_varf(float variable){ void Monitoring_Error(char * msg){ gpio_put(LED_PIN_ROUGE, 1); -} \ No newline at end of file +} + +void Monitoring_set_erreur_critique(){ + Monitoring_Error(""); + erreur_critique=1; +} + +int Monitoring_get_erreur_critique(){ + return erreur_critique; +} diff --git a/Monitoring.h b/Monitoring.h index c585df0..f75e67e 100644 --- a/Monitoring.h +++ b/Monitoring.h @@ -8,4 +8,6 @@ uint32_t temps_cycle_get_max(); void set_debug_var(uint32_t variable); void set_debug_varf(float variable); void Monitoring_display(); -void Monitoring_Error(char * msg); \ No newline at end of file +void Monitoring_Error(char * msg); +void Monitoring_set_erreur_critique(); +int Monitoring_get_erreur_critique(); \ No newline at end of file diff --git a/Strategie.c b/Strategie.c index e54e2ec..a49c15d 100644 --- a/Strategie.c +++ b/Strategie.c @@ -4,6 +4,7 @@ #include "Commande_vitesse.h" #include "Geometrie_robot.h" #include "Localisation.h" +#include "Monitoring.h" #include "Moteurs.h" #include "Score.h" #include "Strategie_prise_cerises.h" @@ -53,8 +54,6 @@ int Robot_est_dans_zone_cerise_droite(); enum etat_action_t Strategie_aller_cerises_laterales_proches(enum couleur_t couleur, uint32_t step_ms); enum etat_action_t Strategie_aller_cerises_laterales_opposees(enum couleur_t couleur, uint32_t step_ms); -enum etat_homologation_t etat_homologation=STRATEGIE_INIT; - void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ @@ -80,8 +79,13 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ ALLER_PANIER, LANCER_PANIER, DECISION_STRATEGIE, + STRATEGIE_FIN }etat_strategie; + if(Monitoring_get_erreur_critique()){ + etat_strategie=STRATEGIE_FIN; + } + switch(etat_strategie){ case STRATEGIE_INIT: if(couleur == COULEUR_BLEU){ @@ -121,7 +125,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, 500); + Trajet_config(250, 250); Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 156, Localisation_get().angle_radian, angle_fin); @@ -158,7 +162,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, 500); + Trajet_config(250, 250); Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 175, Localisation_get().angle_radian, angle_fin); @@ -185,7 +189,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ break; case ALLER_CERISE_GAUCHE: - Trajet_config(300, 250); + Trajet_config(250, 250); angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150. * DEGRE_EN_RADIAN); if(couleur == COULEUR_BLEU){ if(Strategie_aller_cerises_laterales_proches(couleur, step_ms)== ACTION_TERMINEE){ @@ -199,7 +203,6 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){ etat_strategie = ATTRAPER_CERISE_GAUCHE; - Trajet_config(500, 500); } break; @@ -211,7 +214,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ break; case ALLER_CERISE_DROITE: - Trajet_config(300, 250); + Trajet_config(250, 250); angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150. * DEGRE_EN_RADIAN); if(couleur == COULEUR_BLEU){ if(Strategie_aller_cerises_laterales_opposees(couleur, step_ms)== ACTION_TERMINEE){ @@ -225,7 +228,6 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){ etat_strategie = ATTRAPER_CERISE_GAUCHE; - Trajet_config(500, 500); } break; @@ -298,14 +300,14 @@ enum etat_action_t Strategie_aller_cerises_laterales_proches(enum couleur_t coul Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, 740, 3000 - 550, 510, 3000 - 1580, - 180, 1400, + 500, 1400, Localisation_get().angle_radian, angle_fin); }else{ angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, 30. * DEGRE_EN_RADIAN); Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, 2000 - 740, 3000 - 550, 2000 - 510, 3000 - 1580, - 2000 - 180, 1400, + 2000 - 500, 1400, Localisation_get().angle_radian, angle_fin); } @@ -338,7 +340,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(500, 500); + Trajet_config(250, 250); // Definition des trajectoires if(couleur == COULEUR_BLEU){ @@ -552,7 +554,7 @@ enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t ste if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){ etat_lance_balles_dans_panier = LANCE_DANS_PANIER; - Trajet_config(500, 500); + Trajet_config(250, 250); } break; diff --git a/Strategie.h b/Strategie.h index e48515f..2a7d203 100644 --- a/Strategie.h +++ b/Strategie.h @@ -25,25 +25,6 @@ enum couleur_t{ COULEUR_INCONNUE, }; -extern enum etat_homologation_t{ - STRATEGIE_INIT, - ATTENTE_TIRETTE, - APPROCHE_CERISE_1_A, - APPROCHE_CERISE_1_B, - ATTRAPE_CERISE_1, - APPROCHE_PANIER_1, - APPROCHE_PANIER_2, - CALAGE_PANIER_1, - RECULE_PANIER, - TOURNE_PANIER, - LANCE_DANS_PANIER, - APPROCHE_CERISE_2, - ATTRAPPE_CERISE_2, - APPROCHE_PANIER_3, - STRATEGIE_FIN, - -}etat_homologation; - enum evitement_t{ SANS_EVITEMENT, PAUSE_DEVANT_OBSTACLE, diff --git a/Test_strategie.c b/Test_strategie.c index 300aedf..e0ed276 100644 --- a/Test_strategie.c +++ b/Test_strategie.c @@ -51,7 +51,6 @@ void affichage_test_strategie(){ printf(">tirette:%ld:%d\n", temps, attente_tirette()); - 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()); diff --git a/gyro_ADXRS453.c b/gyro_ADXRS453.c index 43cf995..1a13dfb 100644 --- a/gyro_ADXRS453.c +++ b/gyro_ADXRS453.c @@ -64,19 +64,21 @@ int gyro_get_sensor_data(uint16_t tampon_envoi[], uint8_t tampon_reception[]){ //printf("SPI ERROR\n"); return 1; }else{ - /*while(1){ + 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); /*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; }