From 49778b85e62600c9c8259daa77bfd000422fae74 Mon Sep 17 00:00:00 2001 From: Samuel Date: Wed, 17 May 2023 19:01:21 +0200 Subject: [PATCH] WIP --- Holonome2023.c | 2 +- Monitoring.c | 7 +++++++ Monitoring.h | 3 ++- Strategie.c | 16 ++++++++-------- gyro_ADXRS453.c | 10 ++++++---- 5 files changed, 24 insertions(+), 14 deletions(-) diff --git a/Holonome2023.c b/Holonome2023.c index 5f77760..ff1ef8b 100644 --- a/Holonome2023.c +++ b/Holonome2023.c @@ -30,7 +30,7 @@ #include "Trajet.h" const uint LED_PIN = 25; -const uint LED_PIN_ROUGE = 28; +#define LED_PIN_ROUGE 28 const uint LED_PIN_NE_PAS_UTILISER = 22; uint temps_cycle; diff --git a/Monitoring.c b/Monitoring.c index 03e148d..7d2862f 100644 --- a/Monitoring.c +++ b/Monitoring.c @@ -5,6 +5,8 @@ #include "Asser_Moteurs.h" #include "i2c_annexe.h" +#define LED_PIN_ROUGE 28 + uint32_t temps_cycle_min = UINT32_MAX; uint32_t temps_cycle_max=0; int lock=0; @@ -76,4 +78,9 @@ void set_debug_var(uint32_t variable){ void set_debug_varf(float variable){ debug_varf = variable; +} + +void Monitoring_Error(char * msg){ + gpio_put(LED_PIN_ROUGE, 1); + } \ No newline at end of file diff --git a/Monitoring.h b/Monitoring.h index 9cb2968..c585df0 100644 --- a/Monitoring.h +++ b/Monitoring.h @@ -7,4 +7,5 @@ uint32_t temps_cycle_get_min(); uint32_t temps_cycle_get_max(); void set_debug_var(uint32_t variable); void set_debug_varf(float variable); -void Monitoring_display(); \ No newline at end of file +void Monitoring_display(); +void Monitoring_Error(char * msg); \ No newline at end of file diff --git a/Strategie.c b/Strategie.c index f496888..37adc8e 100644 --- a/Strategie.c +++ b/Strategie.c @@ -86,8 +86,8 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ 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_2 = { .priorite = 2, .etat = A_FAIRE, .cible = CERISE_HAUT}; + struct objectif_t objectif_3 = { .priorite = 1, .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; @@ -96,8 +96,8 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ }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_2 = { .priorite = 2, .etat = A_FAIRE, .cible = CERISE_HAUT}; + struct objectif_t objectif_3 = { .priorite = 1, .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; @@ -211,11 +211,11 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ Trajet_config(300, 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){ + if(Strategie_aller_cerises_laterales_opposees(couleur, step_ms)== ACTION_TERMINEE){ etat_strategie = ATTRAPER_CERISE_DROITE; } }else{ - if(Strategie_aller_cerises_laterales_opposees(couleur, step_ms)== ACTION_TERMINEE){ + if(Strategie_aller_cerises_laterales_proches(couleur, step_ms)== ACTION_TERMINEE){ etat_strategie = ATTRAPER_CERISE_DROITE; } } @@ -295,14 +295,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, 1500, + 180, 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, 1500, + 2000 - 180, 1400, Localisation_get().angle_radian, angle_fin); } diff --git a/gyro_ADXRS453.c b/gyro_ADXRS453.c index 9e7f67f..25c0fca 100644 --- a/gyro_ADXRS453.c +++ b/gyro_ADXRS453.c @@ -1,4 +1,5 @@ #include "gyro_ADXRS453.h" +#include "Monitoring.h" #include "spi_nb.h" #include @@ -29,13 +30,13 @@ int gyro_spi_wr_32bits(uint16_t *transmit_buffer, uint8_t *recieve_buffer){ cs_select(); if(spi_nb_write_data(spi0, (uint16_t*) transmit_buffer, 4) == SPI_ERR_TRANSMIT_FIFO_FULL){ - puts("gyro_spi_wr_32bits: SPI_ERR_TRANSMIT_FIFO_FULL"); + //puts("gyro_spi_wr_32bits: SPI_ERR_TRANSMIT_FIFO_FULL"); }else{ while(spi_nb_busy(spi0)); nb_recu = spi_nb_read_data_8bits(spi0, recieve_buffer); } if(nb_recu != 4){ - puts("gyro_spi_wr_32bits: nb_recu incohérent"); + //puts("gyro_spi_wr_32bits: nb_recu incohérent"); } cs_deselect(); } @@ -56,8 +57,9 @@ 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"); return 1; } if(Gyro_SensorData.ST != 0x1){