Keuronde/RPiPico-Holonome2023
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Visualisation des capteurs VL53L1X ignorés + corrections dans la gestion des cônes de détection.

Browse Source 
- Cas de test spécifique
- La distance des cônes n'est pas encore prise en compte
This commit is contained in:
Samuel 2023-04-17 22:16:14 +02:00
parent e1a662d9e7
commit 13ea083670
4 changed files with 63 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
Balise_VL53L1X.c
View File

@ -14,12 +14,12 @@ uint8_t distance_capteur_cm[NB_CAPTEURS];
struct capteur_VL53L1X_t{
uint8_t distance_lue_cm; // Distance entre le capteur et l'obstacle.
uint8_t distance_obstacle_robot_mm; // Distance entre l'obstacle et le centre du robot.
double distance_obstacle_robot_mm; // Distance entre l'obstacle et le centre du robot.
double angle_ref_robot; // Orientation du capteur dans le référentiel du robot
double angle_ref_terrain; // Orientation du capteur dans le référentiel du terrain
double angle_ref_terrain_min; // Cone de détection du capteur (min)
double angle_ref_terrain_max; // Cone de détection du capteur (max)
uint donnee_valide;
uint donnee_valide; // L'obstacle détecté est dans le terrain et n'est pas dans le robot
uint donnee_ignore; // Le capteur est ignoré car derrière le robot.
}capteurs_VL53L1X[NB_CAPTEURS];
@ -42,14 +42,14 @@ void actualise_VL53L1X(struct capteur_VL53L1X_t * capteur_VL53L1X, uint8_t dista
while(capteur_VL53L1X->angle_ref_terrain > M_PI){
capteur_VL53L1X->angle_ref_terrain -= 2* M_PI;
}
while(capteur_VL53L1X->angle_ref_terrain < 2* -M_PI){
while(capteur_VL53L1X->angle_ref_terrain < -M_PI){
capteur_VL53L1X->angle_ref_terrain += 2* M_PI;
}
capteur_VL53L1X->distance_lue_cm = distance_capteur_cm;
capteur_VL53L1X->distance_obstacle_robot_mm = 10 * (distance_capteur_cm + DISTANCE_CAPTEUR_CENTRE_ROBOT_CM);
capteur_VL53L1X->angle_ref_terrain_min = capteur_VL53L1X->angle_ref_terrain - DEMI_CONE_CAPTEUR_RADIAN;
capteur_VL53L1X->angle_ref_terrain_max = capteur_VL53L1X->angle_ref_terrain + DEMI_CONE_CAPTEUR_RADIAN;
capteur_VL53L1X->angle_ref_terrain_min = Geometrie_get_angle_normalisee(capteur_VL53L1X->angle_ref_terrain - DEMI_CONE_CAPTEUR_RADIAN);
capteur_VL53L1X->angle_ref_terrain_max = Geometrie_get_angle_normalisee(capteur_VL53L1X->angle_ref_terrain + DEMI_CONE_CAPTEUR_RADIAN);
invalide_obstacle(capteur_VL53L1X, position_robot);
@ -61,8 +61,8 @@ void invalide_obstacle(struct capteur_VL53L1X_t *capteur_VL53L1X, struct positio
// Positionne l'obstacle sur le terrain
struct position_t position_obstacle;
//printf("Angle:%.1f\n",capteur_VL53L1X->angle_ref_terrain);
position_obstacle.x_mm = position_robot.x_mm + cos(capteur_VL53L1X->angle_ref_terrain)* (double)(capteur_VL53L1X->distance_obstacle_robot_mm);
position_obstacle.y_mm = position_robot.y_mm + sin(capteur_VL53L1X->angle_ref_terrain)* (double)(capteur_VL53L1X->distance_obstacle_robot_mm * 10);
position_obstacle.x_mm = position_robot.x_mm + cos(capteur_VL53L1X->angle_ref_terrain)* capteur_VL53L1X->distance_obstacle_robot_mm;
position_obstacle.y_mm = position_robot.y_mm + sin(capteur_VL53L1X->angle_ref_terrain)* capteur_VL53L1X->distance_obstacle_robot_mm;
capteur_VL53L1X->donnee_valide=1;
// Si la distance vaut 0, à invalider
@ -118,6 +118,7 @@ uint8_t Balise_VL53L1X_get_capteur_cm(uint8_t capteur){
/// @return
double Balise_VL53L1X_get_distance_obstacle_mm(double angle_avancement_radiant){
const uint8_t NB_CONE=3;
uint16_t masque_led=0;
struct cone_t{
double distance_mm;
double angle;
@ -137,11 +138,23 @@ double Balise_VL53L1X_get_distance_obstacle_mm(double angle_avancement_radiant){
for(uint8_t capteur=0; capteur<NB_CAPTEURS; capteur++){
capteurs_VL53L1X[capteur].donnee_ignore = 1;
for(uint8_t cone_index=0; cone_index<NB_CONE; cone_index++){
/*printf("capteur:%d\n", capteur);
printf("capteur_angle_min:%f\n", capteurs_VL53L1X[capteur].angle_ref_terrain_min);
printf("capteur_angle_max:%f\n", capteurs_VL53L1X[capteur].angle_ref_terrain_max);
printf("cone angel min:%f\n", angle_avancement_radiant - cone[cone_index].angle);
printf("cone angel max:%f\n", angle_avancement_radiant + cone[cone_index].angle);*/
//On test si le capteur détecte dans la plage du cône
if(Geometrie_intersecte_plage_angle(
angle_avancement_radiant - cone[cone_index].angle, angle_avancement_radiant + cone[cone_index].angle,
capteurs_VL53L1X[capteur].angle_ref_terrain_min, capteurs_VL53L1X[capteur].angle_ref_terrain_max)){
// Si l'obstacle est sur le terrain
if(capteurs_VL53L1X[capteur].donnee_valide){
if(capteurs_VL53L1X[capteur].distance_obstacle_robot_mm < distance_minimale){
// Si la distance est plus petite que la distance minimale actuelle
// Si la distance est plus petite que le cône en question...
if(capteurs_VL53L1X[capteur].distance_obstacle_robot_mm < distance_minimale /*&&
capteurs_VL53L1X[capteur].distance_obstacle_robot_mm < cone[cone_index].distance_mm*/){
// On retient cette distance comme étant notre distance minimale
distance_minimale = capteurs_VL53L1X[capteur].distance_obstacle_robot_mm;
}
}
@ -151,6 +164,14 @@ double Balise_VL53L1X_get_distance_obstacle_mm(double angle_avancement_radiant){
}
}
// On éteint les LEDs qui correspondent aux capteurs ignorés
for(uint8_t capteur=0; capteur<NB_CAPTEURS; capteur++){
if(capteurs_VL53L1X[capteur].donnee_ignore == 1){
masque_led |= 1 << capteur;
}
}
i2c_annexe_couleur_balise(0, masque_led);
// On enlève le rayon du robot et la taille du robot adverse
if(distance_minimale < DISTANCE_OBSTACLE_IGNORE_MM){
distance_minimale -= (RAYON_ROBOT + RAYON_ROBOT_ADVERSE_MM);
@ -160,4 +181,4 @@ double Balise_VL53L1X_get_distance_obstacle_mm(double angle_avancement_radiant){
}
return distance_minimale;
}
}

3
Balise_VL53L1X.h
View File

@ -2,4 +2,5 @@
void Balise_VL53L1X_init(void);
void Balise_VL53L1X_gestion(void);
uint8_t Balise_VL53L1X_get_min_distance(void);
uint8_t Balise_VL53L1X_get_capteur_cm(uint8_t capteur);
uint8_t Balise_VL53L1X_get_capteur_cm(uint8_t capteur);
double Balise_VL53L1X_get_distance_obstacle_mm(double angle_avancement_radiant);

2
Geometrie.c
View File

@ -8,7 +8,7 @@ double Geometrie_get_angle_normalisee(double angle){
while(angle > M_PI){
angle -= 2* M_PI;
}
while(angle < 2* -M_PI){
while(angle < -M_PI){
angle += 2* M_PI;
}
return angle;

30
Test.c
View File

@ -60,6 +60,7 @@ int test_aller_retour();
void test_trajectoire_teleplot();
int test_capteurs_balise(void);
int test_geometrie(void);
int test_angle_balise(void);
// Mode test : renvoie 0 pour quitter le mode test
@ -161,6 +162,11 @@ int mode_test(){
while(test_geometrie());
break;
case 'O':
case 'o':
while(test_angle_balise());
break;
case 'T':
case 't':
while(test_trajectoire());
@ -1368,4 +1374,28 @@ int test_geometrie(){
Geometrie_compare_angle(angle*DEGRE_EN_RADIAN, angle_min*DEGRE_EN_RADIAN, angle_max*DEGRE_EN_RADIAN));
return 0;
}
int test_angle_balise(void){
i2c_maitre_init();
Balise_VL53L1X_init();
Localisation_set(1000,1500,0);
int lettre;
double distance, angle=0;
do{
i2c_gestion(i2c0);
i2c_annexe_gestion();
Balise_VL53L1X_gestion();
distance = Balise_VL53L1X_get_distance_obstacle_mm(angle);
printf(">distance_obstacle:%3.0f\n", distance);
lettre = getchar_timeout_us(0);
}while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);
return 0;
}