From f08edb590e60b3efaadf347ba1a616cccc663230 Mon Sep 17 00:00:00 2001 From: Samuel Date: Tue, 7 May 2024 09:56:57 +0200 Subject: [PATCH] =?UTF-8?q?arr=C3=AAt=20devant=20obstacle=20+=20structure?= =?UTF-8?q?=20pour=20g=C3=A9rer=20les=206=20robots=20de=202=20couleurs?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .vscode/settings.json | 4 +- QEI.c | 4 +- Trajet.c | 5 +- Trajet.h | 2 +- VL53L8_2024.c | 25 ++++- VL53L8_2024.h | 2 + main.c | 212 ++++++++++++++++++++++++++---------------- 7 files changed, 170 insertions(+), 84 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 64aec37..4cac3bc 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -6,6 +6,8 @@ "asser_moteurs.h": "c", "localisation.h": "c", "vl53l8_2024.h": "c", - "trajet.h": "c" + "trajet.h": "c", + "trajectoire.h": "c", + "compare": "c" } } \ No newline at end of file diff --git a/QEI.c b/QEI.c index 4d8881f..d035df6 100644 --- a/QEI.c +++ b/QEI.c @@ -17,8 +17,8 @@ // Nombre d'impulsions par tour de roue : 8000 // Impulsion / mm : 42,44 -//#define IMPULSION_PAR_MM (12.45f) -#define IMPULSION_PAR_MM (16.45f) +#define IMPULSION_PAR_MM (12.45f) + struct QEI_t QEI_A, QEI_B; diff --git a/Trajet.c b/Trajet.c index 4dc9ef0..3f02ebb 100644 --- a/Trajet.c +++ b/Trajet.c @@ -141,12 +141,15 @@ float Trajet_calcul_vitesse(float pas_de_temps_s){ vitesse_max_contrainte = 0; } - distance_contrainte_obstacle = Trajet_get_obstacle_mm(); + /*distance_contrainte_obstacle = Trajet_get_obstacle_mm(); if(distance_contrainte_obstacle != DISTANCE_INVALIDE){ vitesse_max_contrainte_obstacle = sqrtf(2 * acceleration_mm_ss_obstacle * distance_contrainte_obstacle); if(vitesse_max_contrainte_obstacle < vitesse_max_contrainte){ vitesse_max_contrainte = vitesse_max_contrainte_obstacle; } + }*/ + if(Trajet_get_obstacle_mm() < 50){ + vitesse = 0; } diff --git a/Trajet.h b/Trajet.h index c0453ec..57462c6 100644 --- a/Trajet.h +++ b/Trajet.h @@ -10,7 +10,7 @@ enum etat_trajet_t{ }; // Vitesse et acceleration pour translation pure (en mm/s et mm/s²) -#define TRAJECT_CONFIG_RAPIDE 300, 600 +#define TRAJECT_CONFIG_RAPIDE 300, 1200 // Vitesse et acceleration pour un mouvement complexe (en mm et mm/s²) #define TRAJECT_CONFIG_AVANCE_ET_TOURNE 300, 500 // Vitesse et acceleration - standard (en mm et mm/s²) diff --git a/VL53L8_2024.c b/VL53L8_2024.c index c12688c..18054b0 100644 --- a/VL53L8_2024.c +++ b/VL53L8_2024.c @@ -1,4 +1,5 @@ #include "vl53l8cx_api.h" +#include "Trajet.h" #include int masque[64]={ @@ -135,6 +136,7 @@ void VL53L8_lecture(VL53L8CX_Configuration * Dev, VL53L8CX_ResultsData * Results } // Affichage des données + /* printf(">distance:"); for(int row=0; row < 8; row++){ for(int col=0; col < 8; col++){ @@ -142,4 +144,25 @@ void VL53L8_lecture(VL53L8CX_Configuration * Dev, VL53L8CX_ResultsData * Results } } printf("\n"); -} \ No newline at end of file + */ +} + +int VL53L8_min_distance(VL53L8CX_ResultsData Results, float *distance){ + uint16_t mesure1, mesure2; + int min_distance = 2000; + int col, row; + for(col=0; col<8; col++){ + for(row=4; row<=6; row++){ + if(min_distance > Results.distance_mm[col + 8*row]){ + min_distance = Results.distance_mm[col + 8*row]; + } + } + } + *distance = 0; + if(min_distance-50 > 0){ + *distance = min_distance-50; + } + + +} + diff --git a/VL53L8_2024.h b/VL53L8_2024.h index 641b9ee..f489b15 100644 --- a/VL53L8_2024.h +++ b/VL53L8_2024.h @@ -2,3 +2,5 @@ void VL53L8_init(VL53L8CX_Configuration * Dev); void VL53L8_lecture(VL53L8CX_Configuration * Dev, VL53L8CX_ResultsData * Results); +int VL53L8_min_distance(VL53L8CX_ResultsData Results, float *distance); + diff --git a/main.c b/main.c index 2f22191..fe97f9f 100644 --- a/main.c +++ b/main.c @@ -10,6 +10,7 @@ #include "Asser_Position.h" #include "Asser_Moteurs.h" #include "Commande_vitesse.h" +#include "i2c_maitre.h" #include "Localisation.h" #include "Moteurs.h" #include "Temps.h" @@ -25,7 +26,11 @@ #define TIRETTE_PIN 6 #define COULEUR_PIN 4 +#define COULEUR_BLEU 1 +#define COULEUR_JAUNE 0 + void affichage(void); +void gestion_affichage(void); void tension_batterie_init(void); uint16_t tension_batterie_lire(void); @@ -36,6 +41,8 @@ int get_tirette(void); int get_couleur(void); void configure_trajet(int identifiant, int couleur); +void gestion_VL53L8CX(void); + uint32_t step_ms=1; float distance1_mm=0, distance2_mm=0; @@ -60,17 +67,7 @@ void main(void) Trajet_init(); i2c_maitre_init(); - VL53L8_init(&Dev); - sleep_ms(100); - VL53L8_lecture( &Dev, &Results); // une première lecture - uint8_t status, isReady; - while(1){ - status = vl53l8cx_check_data_ready(&Dev, &isReady); - if(isReady){ - VL53L8_lecture( &Dev, &Results); - } - - } + uint32_t temps_ms = Temps_get_temps_ms(); uint32_t temps_depart_ms; @@ -83,24 +80,13 @@ void main(void) gpio_put(LED1PIN, 1); - multicore_launch_core1(affichage); + //multicore_launch_core1(gestion_affichage); + multicore_launch_core1(gestion_VL53L8CX); + sleep_ms(4000); + - - /*Localisation_set(1130, 2000-63, M_PI); - struct trajectoire_t trajectoire; - Trajectoire_bezier(&trajectoire, 1122, 2000-63, 905, 2000-63, - 606, 2000-590, 225, 2000-225, M_PI, M_PI); - Trajet_config(TRAJECT_CONFIG_RAPIDE); - Trajet_debut_trajectoire(trajectoire);*/ - - /*Localisation_set(0, 0, 0); - struct trajectoire_t trajectoire; - Trajectoire_droite(&trajectoire, 0,0,1000,0, M_PI, M_PI); - Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE); - Trajet_debut_trajectoire(trajectoire);*/ - - configure_trajet(identifiant_lire(), 0); + configure_trajet(identifiant_lire(), get_couleur()); float vitesse_init =300; @@ -125,13 +111,17 @@ void main(void) temps_ms = Temps_get_temps_ms(); if(temps_ms % step_ms == 0){ QEI_update(); - Localisation_gestion(); - if(etat_trajet != TRAJET_TERMINE){ - etat_trajet = Trajet_avance((float)step_ms/1000.); + Localisation_gestion(); + if(Trajet_get_obstacle_mm() < 50){ + Moteur_Stop(); }else{ - Asser_Position_maintien(); + if(etat_trajet != TRAJET_TERMINE){ + etat_trajet = Trajet_avance((float)step_ms/1000.); + }else{ + Asser_Position_maintien(); + } + AsserMoteur_Gestion(step_ms); } - AsserMoteur_Gestion(step_ms); } } @@ -139,24 +129,53 @@ void main(void) } } -extern float delta_x_mm, delta_y_mm, delta_orientation_radian; -void affichage(void){ + +/// @brief Obtient la distance de l'obstacle le plus proche. +/// @param +void gestion_VL53L8CX(void){ + VL53L8CX_ResultsData Results; + float distance_obstacle; + VL53L8_init(&Dev); + sleep_ms(100); + VL53L8_lecture( &Dev, &Results); // une première lecture + uint8_t status, isReady; while(1){ - /*printf(">m1:%f\n>m2:%f\n", AsserMoteur_getVitesse_mm_s(MOTEUR_A, step_ms), AsserMoteur_getVitesse_mm_s(MOTEUR_B, step_ms) ); - printf(">m1_c:%f\n>m2_c:%f\n", AsserMoteur_getConsigne_mm_s(MOTEUR_A), AsserMoteur_getConsigne_mm_s(MOTEUR_B) );*/ - printf(">pos_x:%.1f\n>pos_y:%.1f\n>pos_angle:%.1f\n", Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian); - //printf(">abscisse:%f\n",abscisse); - - struct position_t position_actuelle; - position_actuelle = Localisation_get(); - printf(">delta_orientation_radian:%.2f\n>angle_delta:%.2f\n",delta_orientation_radian, atan2f(delta_y_mm, delta_x_mm)); - printf(">pos_x:%.2f\n>pos_y:%.2f\n", position_actuelle.x_mm, position_actuelle.y_mm); - printf(">con_x:%.2f\n>con_y:%.2f\n", point.point_xy.x, point.point_xy.y); - - //printf(">couleur:%d\n>id:%d\n", get_couleur(), identifiant_lire()); + status = vl53l8cx_check_data_ready(&Dev, &isReady); + if(isReady){ + VL53L8_lecture( &Dev, &Results); + VL53L8_min_distance(Results, &distance_obstacle); + Trajet_set_obstacle_mm(distance_obstacle); + } + affichage(); } } + +extern float delta_x_mm, delta_y_mm, delta_orientation_radian; + +void gestion_affichage(void){ + while(1){ + affichage(); + } +} + +void affichage(void){ +/*printf(">m1:%f\n>m2:%f\n", AsserMoteur_getVitesse_mm_s(MOTEUR_A, step_ms), AsserMoteur_getVitesse_mm_s(MOTEUR_B, step_ms) ); + printf(">m1_c:%f\n>m2_c:%f\n", AsserMoteur_getConsigne_mm_s(MOTEUR_A), AsserMoteur_getConsigne_mm_s(MOTEUR_B) );*/ + printf(">pos_x:%.1f\n>pos_y:%.1f\n>pos_angle:%.1f\n", Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian); + printf(">distance_obstacle:%f\n",Trajet_get_obstacle_mm()); + + printf(">abscisse:%f\n",abscisse); + + struct position_t position_actuelle; + position_actuelle = Localisation_get(); + printf(">delta_orientation_radian:%.2f\n>angle_delta:%.2f\n",delta_orientation_radian, atan2f(delta_y_mm, delta_x_mm)); + printf(">pos_x:%.2f\n>pos_y:%.2f\n", position_actuelle.x_mm, position_actuelle.y_mm); + printf(">con_x:%.2f\n>con_y:%.2f\n", point.point_xy.x, point.point_xy.y); + + printf(">couleur:%d\n>id:%d\n", get_couleur(), identifiant_lire()); +} + void tension_batterie_init(void){ adc_init(); adc_gpio_init(28); // Analog_2 @@ -199,11 +218,11 @@ int get_tirette(void){ } int get_couleur(void){ - return !gpio_get(COULEUR_PIN); + return gpio_get(COULEUR_PIN); } -/// @brief !! Arg la GPIO 26 ne répond pas ! -/// @return +/// @brief !! Arg la GPIO 26 ne répond pas ! => Réglé ADC_VREF était à la masse +/// @return identifiant du robot (PDI switch) uint identifiant_lire(){ return (gpio_get(21) << 2)+ (gpio_get(22) << 1) + gpio_get(26); } @@ -211,36 +230,73 @@ uint identifiant_lire(){ void configure_trajet(int identifiant, int couleur){ struct trajectoire_t trajectoire; - - - switch (identifiant) - { - case 0: - break; - case 1: - Localisation_set(1465, 2000-63, M_PI); - Trajectoire_bezier(&trajectoire, 1465, 2000-63, 1260, 2000-63, - 430, 1240, 430, 2000, M_PI, M_PI); - break; - case 2: - break; - case 3: - Localisation_set(1130, 2000-63, M_PI); - Trajectoire_bezier(&trajectoire, 1122, 2000-63, 905, 2000-63, - 606, 2000-590, 225, 2000-225, M_PI, M_PI); - break; - case 4: - break; - case 5: - break; - case 6: - break; - case 7: - break; - - default: - break; + + switch(couleur){ + case COULEUR_JAUNE: + switch (identifiant) + { + case 0: + break; + case 1: + Localisation_set(1465, 2000-63, M_PI); + Trajectoire_bezier(&trajectoire, 3000-1465, 2000-63, 3000-1260, 2000-63, + 3000-600, 1400, 3000-0, 2000, M_PI, M_PI); + break; + case 2: + break; + case 3: + Localisation_set(3000 - 1130, 2000-63, 0); + Trajectoire_bezier(&trajectoire, 3000-1122, 2000-63, 3000-905, 2000-63, + 3000-606, 2000-590, 3000-225, 2000-225, 0, 0); + break; + case 4: + break; + case 5: + break; + case 6: + break; + case 7: + break; + + default: + break; + } + break; + + case COULEUR_BLEU: + switch (identifiant) + { + case 0: + break; + case 1: + Localisation_set(1465, 2000-63, M_PI); + Trajectoire_bezier(&trajectoire, 1465, 2000-63, 1260, 2000-63, + 600, 1400, 0, 2000, M_PI, M_PI); + break; + case 2: + break; + case 3: + Localisation_set(1130, 2000-63, M_PI); + Trajectoire_bezier(&trajectoire, 1122, 2000-63, 905, 2000-63, + 606, 2000-590, 225, 2000-225, M_PI, M_PI); + break; + case 4: + break; + case 5: + break; + case 6: + break; + case 7: + break; + + default: + break; + } + break; } + + + Trajet_config(TRAJECT_CONFIG_RAPIDE); Trajet_debut_trajectoire(trajectoire);