Keuronde/PAMI_2024
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

arrêt devant obstacle + structure pour gérer les 6 robots de 2 couleurs

Browse Source
This commit is contained in:
Samuel 2024-05-07 09:56:57 +02:00
parent 3ebac86f82
commit f08edb590e
7 changed files with 170 additions and 84 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
.vscode/settings.json vendored
View File

@ -6,6 +6,8 @@
"asser_moteurs.h": "c", "asser_moteurs.h": "c",
"localisation.h": "c", "localisation.h": "c",
"vl53l8_2024.h": "c", "vl53l8_2024.h": "c",
"trajet.h": "c" "trajet.h": "c",
"trajectoire.h": "c",
"compare": "c"
} }
} }

4
QEI.c
View File

@ -17,8 +17,8 @@
// Nombre d'impulsions par tour de roue : 8000 // Nombre d'impulsions par tour de roue : 8000
// Impulsion / mm : 42,44 // Impulsion / mm : 42,44
//#define IMPULSION_PAR_MM (12.45f) #define IMPULSION_PAR_MM (12.45f)
#define IMPULSION_PAR_MM (16.45f)
struct QEI_t QEI_A, QEI_B; struct QEI_t QEI_A, QEI_B;

5
Trajet.c
View File

@ -141,12 +141,15 @@ float Trajet_calcul_vitesse(float pas_de_temps_s){
vitesse_max_contrainte = 0; vitesse_max_contrainte = 0;
} }
distance_contrainte_obstacle = Trajet_get_obstacle_mm(); /*distance_contrainte_obstacle = Trajet_get_obstacle_mm();
if(distance_contrainte_obstacle != DISTANCE_INVALIDE){ if(distance_contrainte_obstacle != DISTANCE_INVALIDE){
vitesse_max_contrainte_obstacle = sqrtf(2 * acceleration_mm_ss_obstacle * distance_contrainte_obstacle); vitesse_max_contrainte_obstacle = sqrtf(2 * acceleration_mm_ss_obstacle * distance_contrainte_obstacle);
if(vitesse_max_contrainte_obstacle < vitesse_max_contrainte){ if(vitesse_max_contrainte_obstacle < vitesse_max_contrainte){
vitesse_max_contrainte = vitesse_max_contrainte_obstacle; vitesse_max_contrainte = vitesse_max_contrainte_obstacle;
} }
}*/
if(Trajet_get_obstacle_mm() < 50){
vitesse = 0;
} }

2
Trajet.h
View File

@ -10,7 +10,7 @@ enum etat_trajet_t{
}; };
// Vitesse et acceleration pour translation pure (en mm/s et mm/s²) // 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²) // Vitesse et acceleration pour un mouvement complexe (en mm et mm/s²)
#define TRAJECT_CONFIG_AVANCE_ET_TOURNE 300, 500 #define TRAJECT_CONFIG_AVANCE_ET_TOURNE 300, 500
// Vitesse et acceleration - standard (en mm et mm/s²) // Vitesse et acceleration - standard (en mm et mm/s²)

25
VL53L8_2024.c
View File

@ -1,4 +1,5 @@
#include "vl53l8cx_api.h" #include "vl53l8cx_api.h"
#include "Trajet.h"
#include <stdio.h> #include <stdio.h>
int masque[64]={ int masque[64]={
@ -135,6 +136,7 @@ void VL53L8_lecture(VL53L8CX_Configuration * Dev, VL53L8CX_ResultsData * Results
} }
// Affichage des données // Affichage des données
/*
printf(">distance:"); printf(">distance:");
for(int row=0; row < 8; row++){ for(int row=0; row < 8; row++){
for(int col=0; col < 8; col++){ for(int col=0; col < 8; col++){
@ -142,4 +144,25 @@ void VL53L8_lecture(VL53L8CX_Configuration * Dev, VL53L8CX_ResultsData * Results
} }
} }
printf("\n"); printf("\n");
} */
}
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;
}
}

2
VL53L8_2024.h
View File

@ -2,3 +2,5 @@
void VL53L8_init(VL53L8CX_Configuration * Dev); void VL53L8_init(VL53L8CX_Configuration * Dev);
void VL53L8_lecture(VL53L8CX_Configuration * Dev, VL53L8CX_ResultsData * Results); void VL53L8_lecture(VL53L8CX_Configuration * Dev, VL53L8CX_ResultsData * Results);
int VL53L8_min_distance(VL53L8CX_ResultsData Results, float *distance);

212
main.c
View File

@ -10,6 +10,7 @@
#include "Asser_Position.h" #include "Asser_Position.h"
#include "Asser_Moteurs.h" #include "Asser_Moteurs.h"
#include "Commande_vitesse.h" #include "Commande_vitesse.h"
#include "i2c_maitre.h"
#include "Localisation.h" #include "Localisation.h"
#include "Moteurs.h" #include "Moteurs.h"
#include "Temps.h" #include "Temps.h"
@ -25,7 +26,11 @@
#define TIRETTE_PIN 6 #define TIRETTE_PIN 6
#define COULEUR_PIN 4 #define COULEUR_PIN 4
#define COULEUR_BLEU 1
#define COULEUR_JAUNE 0
void affichage(void); void affichage(void);
void gestion_affichage(void);
void tension_batterie_init(void); void tension_batterie_init(void);
uint16_t tension_batterie_lire(void); uint16_t tension_batterie_lire(void);
@ -36,6 +41,8 @@ int get_tirette(void);
int get_couleur(void); int get_couleur(void);
void configure_trajet(int identifiant, int couleur); void configure_trajet(int identifiant, int couleur);
void gestion_VL53L8CX(void);
uint32_t step_ms=1; uint32_t step_ms=1;
float distance1_mm=0, distance2_mm=0; float distance1_mm=0, distance2_mm=0;
@ -60,17 +67,7 @@ void main(void)
Trajet_init(); Trajet_init();
i2c_maitre_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_ms = Temps_get_temps_ms();
uint32_t temps_depart_ms; uint32_t temps_depart_ms;
@ -83,24 +80,13 @@ void main(void)
gpio_put(LED1PIN, 1); gpio_put(LED1PIN, 1);
multicore_launch_core1(affichage); //multicore_launch_core1(gestion_affichage);
multicore_launch_core1(gestion_VL53L8CX);
sleep_ms(4000);
configure_trajet(identifiant_lire(), get_couleur());
/*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);
float vitesse_init =300; float vitesse_init =300;
@ -125,13 +111,17 @@ void main(void)
temps_ms = Temps_get_temps_ms(); temps_ms = Temps_get_temps_ms();
if(temps_ms % step_ms == 0){ if(temps_ms % step_ms == 0){
QEI_update(); QEI_update();
Localisation_gestion(); Localisation_gestion();
if(etat_trajet != TRAJET_TERMINE){ if(Trajet_get_obstacle_mm() < 50){
etat_trajet = Trajet_avance((float)step_ms/1000.); Moteur_Stop();
}else{ }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){ 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) ); status = vl53l8cx_check_data_ready(&Dev, &isReady);
printf(">m1_c:%f\n>m2_c:%f\n", AsserMoteur_getConsigne_mm_s(MOTEUR_A), AsserMoteur_getConsigne_mm_s(MOTEUR_B) );*/ if(isReady){
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); VL53L8_lecture( &Dev, &Results);
//printf(">abscisse:%f\n",abscisse); VL53L8_min_distance(Results, &distance_obstacle);
Trajet_set_obstacle_mm(distance_obstacle);
struct position_t position_actuelle; }
position_actuelle = Localisation_get(); affichage();
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());
} }
} }
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){ void tension_batterie_init(void){
adc_init(); adc_init();
adc_gpio_init(28); // Analog_2 adc_gpio_init(28); // Analog_2
@ -199,11 +218,11 @@ int get_tirette(void){
} }
int get_couleur(void){ int get_couleur(void){
return !gpio_get(COULEUR_PIN); return gpio_get(COULEUR_PIN);
} }
/// @brief !! Arg la GPIO 26 ne répond pas ! /// @brief !! Arg la GPIO 26 ne répond pas ! => Réglé ADC_VREF était à la masse
/// @return /// @return identifiant du robot (PDI switch)
uint identifiant_lire(){ uint identifiant_lire(){
return (gpio_get(21) << 2)+ (gpio_get(22) << 1) + gpio_get(26); 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){ void configure_trajet(int identifiant, int couleur){
struct trajectoire_t trajectoire; struct trajectoire_t trajectoire;
switch(couleur){
switch (identifiant) case COULEUR_JAUNE:
{ switch (identifiant)
case 0: {
break; case 0:
case 1: break;
Localisation_set(1465, 2000-63, M_PI); case 1:
Trajectoire_bezier(&trajectoire, 1465, 2000-63, 1260, 2000-63, Localisation_set(1465, 2000-63, M_PI);
430, 1240, 430, 2000, M_PI, M_PI); Trajectoire_bezier(&trajectoire, 3000-1465, 2000-63, 3000-1260, 2000-63,
break; 3000-600, 1400, 3000-0, 2000, M_PI, M_PI);
case 2: break;
break; case 2:
case 3: break;
Localisation_set(1130, 2000-63, M_PI); case 3:
Trajectoire_bezier(&trajectoire, 1122, 2000-63, 905, 2000-63, Localisation_set(3000 - 1130, 2000-63, 0);
606, 2000-590, 225, 2000-225, M_PI, M_PI); Trajectoire_bezier(&trajectoire, 3000-1122, 2000-63, 3000-905, 2000-63,
break; 3000-606, 2000-590, 3000-225, 2000-225, 0, 0);
case 4: break;
break; case 4:
case 5: break;
break; case 5:
case 6: break;
break; case 6:
case 7: break;
break; case 7:
break;
default:
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_config(TRAJECT_CONFIG_RAPIDE);
Trajet_debut_trajectoire(trajectoire); Trajet_debut_trajectoire(trajectoire);