182 lines
8.0 KiB
C
182 lines
8.0 KiB
C
#include <stdio.h>
|
|
#include "Geometrie_robot.h"
|
|
#include "i2c_annexe.h"
|
|
#include "Localisation.h"
|
|
#include "math.h"
|
|
|
|
#define NB_CAPTEURS 12
|
|
#define DISTANCE_OBSTACLE_IGNORE_MM 2000
|
|
#define DISTANCE_CAPTEUR_CENTRE_ROBOT_CM 4
|
|
#define DISTANCE_TROP_PRES_CM 3
|
|
#define DEMI_CONE_CAPTEUR_RADIAN 0.2225
|
|
|
|
uint8_t distance_capteur_cm[NB_CAPTEURS];
|
|
uint8_t capteur_courant=0; /* capteur en cours d'actualisation */
|
|
|
|
struct capteur_VL53L1X_t{
|
|
uint8_t distance_lue_cm; // Distance entre le capteur et l'obstacle.
|
|
float distance_obstacle_robot_mm; // Distance entre l'obstacle et le centre du robot.
|
|
float angle_ref_robot; // Orientation du capteur dans le référentiel du robot
|
|
float angle_ref_terrain; // Orientation du capteur dans le référentiel du terrain
|
|
float angle_ref_terrain_min; // Cone de détection du capteur (min)
|
|
float angle_ref_terrain_max; // Cone de détection du capteur (max)
|
|
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];
|
|
|
|
void actualise_VL53L1X(struct capteur_VL53L1X_t * capteur_VL53L1X, uint8_t distance_capteur_cm);
|
|
void invalide_obstacle(struct capteur_VL53L1X_t * capteur_VL53L1X, struct position_t position_robot);
|
|
|
|
void Balise_VL53L1X_gestion(){
|
|
i2c_annexe_get_distances(distance_capteur_cm);
|
|
actualise_VL53L1X(&(capteurs_VL53L1X[capteur_courant]), distance_capteur_cm[capteur_courant]);
|
|
capteur_courant++;
|
|
if(capteur_courant == NB_CAPTEURS){
|
|
capteur_courant=0;
|
|
}
|
|
}
|
|
|
|
void actualise_VL53L1X(struct capteur_VL53L1X_t * capteur_VL53L1X, uint8_t distance_capteur_cm){
|
|
struct position_t position_robot;
|
|
position_robot = Localisation_get();
|
|
// Actualisation de l'angle du capteur
|
|
// Maintien de l'angle entre -PI et PI
|
|
capteur_VL53L1X->angle_ref_terrain = Geometrie_get_angle_normalisee(capteur_VL53L1X->angle_ref_robot + position_robot.angle_radian);
|
|
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 = 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);
|
|
|
|
}
|
|
|
|
/// @brief Definit si l'obstable doit être pris en comptre
|
|
/// @param
|
|
void invalide_obstacle(struct capteur_VL53L1X_t *capteur_VL53L1X, struct position_t position_robot){
|
|
// 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 + cosf(capteur_VL53L1X->angle_ref_terrain)* capteur_VL53L1X->distance_obstacle_robot_mm;
|
|
position_obstacle.y_mm = position_robot.y_mm + sinf(capteur_VL53L1X->angle_ref_terrain)* capteur_VL53L1X->distance_obstacle_robot_mm;
|
|
|
|
capteur_VL53L1X->donnee_valide=1;
|
|
// Si la distance vaut 0, à invalider
|
|
if(capteur_VL53L1X->distance_lue_cm <= DISTANCE_TROP_PRES_CM){
|
|
capteur_VL53L1X->donnee_valide=0;
|
|
}
|
|
// Si l'obstacle est à l'extérieur du terrain (on prend 50 mm de marge à l'intérieur du terrain, la balise faisant 100 mm)
|
|
//printf("X:%.1f,Y:%.1f\n", position_obstacle.x_mm, position_obstacle.y_mm);
|
|
if((position_obstacle.x_mm < 50) || (position_obstacle.y_mm < 50) || (position_obstacle.x_mm > 1950) || (position_obstacle.y_mm > 2950))
|
|
{
|
|
capteur_VL53L1X->donnee_valide=0;
|
|
}
|
|
|
|
}
|
|
|
|
void Balise_VL53L1X_init(){
|
|
capteurs_VL53L1X[9].angle_ref_robot = 0;
|
|
capteurs_VL53L1X[10].angle_ref_robot = M_PI/6;
|
|
capteurs_VL53L1X[11].angle_ref_robot = 2*M_PI/6;
|
|
capteurs_VL53L1X[0].angle_ref_robot = 3*M_PI/6;
|
|
capteurs_VL53L1X[1].angle_ref_robot = 4*M_PI/6;
|
|
capteurs_VL53L1X[2].angle_ref_robot = 5*M_PI/6;
|
|
capteurs_VL53L1X[3].angle_ref_robot = 6*M_PI/6;
|
|
capteurs_VL53L1X[4].angle_ref_robot = 7*M_PI/6;
|
|
capteurs_VL53L1X[5].angle_ref_robot = 8*M_PI/6;
|
|
capteurs_VL53L1X[6].angle_ref_robot = 9*M_PI/6;
|
|
capteurs_VL53L1X[7].angle_ref_robot = 10*M_PI/6;
|
|
capteurs_VL53L1X[8].angle_ref_robot = 11*M_PI/6;
|
|
}
|
|
|
|
uint8_t Balise_VL53L1X_get_min_distance(void){
|
|
uint8_t min_distance_cm = DISTANCE_OBSTACLE_IGNORE_MM / 10;
|
|
for(uint8_t capteur=0; capteur<NB_CAPTEURS; capteur++){
|
|
if(capteurs_VL53L1X[capteur].donnee_valide){
|
|
if(min_distance_cm> capteurs_VL53L1X[capteur].distance_lue_cm){
|
|
min_distance_cm = capteurs_VL53L1X[capteur].distance_lue_cm;
|
|
}
|
|
}
|
|
}
|
|
return min_distance_cm;
|
|
}
|
|
|
|
uint8_t Balise_VL53L1X_get_capteur_cm(uint8_t capteur){
|
|
return capteurs_VL53L1X[capteur].distance_lue_cm;
|
|
}
|
|
|
|
/// @brief Renvoi l'obstacle le plus proche en tenant compte de la direction d'avancement du robot dans le référentiel du terrain.
|
|
/// La fonction utilise 3 cônes de détection :
|
|
/// * +/- 22°, à 800 mm
|
|
/// * +/- 50°, à 580 mm
|
|
/// * +/- 90°, à 350 mm
|
|
/// @param angle_avancement_radiant : angle d'avancement du robot entre -PI et PI
|
|
/// @return
|
|
float Balise_VL53L1X_get_distance_obstacle_mm(float angle_avancement_radiant){
|
|
const uint8_t NB_CONE=3;
|
|
uint16_t masque_led=0;
|
|
struct cone_t{
|
|
float distance_mm;
|
|
float angle, angle_min, angle_max;
|
|
} cone[NB_CONE];
|
|
cone[0].angle = 22 * DEGRE_EN_RADIAN;
|
|
cone[0].distance_mm = 1200;
|
|
|
|
cone[1].angle = 50 * DEGRE_EN_RADIAN;
|
|
cone[1].distance_mm = 580;
|
|
|
|
cone[2].angle = 90 * DEGRE_EN_RADIAN;
|
|
cone[2].distance_mm = 350;
|
|
|
|
for(uint8_t cone_index=0; cone_index<NB_CONE; cone_index++){
|
|
cone[cone_index].angle_min = Geometrie_get_angle_normalisee( angle_avancement_radiant - cone[cone_index].angle);
|
|
cone[cone_index].angle_max = Geometrie_get_angle_normalisee( angle_avancement_radiant + cone[cone_index].angle);
|
|
}
|
|
|
|
float angle_min, angle_max;
|
|
float distance_minimale = DISTANCE_OBSTACLE_IGNORE_MM;
|
|
|
|
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++){
|
|
|
|
//On test si le capteur détecte dans la plage du cône
|
|
if(Geometrie_intersecte_plage_angle(
|
|
cone[cone_index].angle_min, cone[cone_index].angle_max,
|
|
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){
|
|
// 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){
|
|
if(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;
|
|
}
|
|
}
|
|
}
|
|
// Le capteur est pris en compte au moins dans un cône
|
|
capteurs_VL53L1X[capteur].donnee_ignore = 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
// 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);
|
|
if(distance_minimale < 0){
|
|
distance_minimale = 0;
|
|
}
|
|
}
|
|
|
|
return distance_minimale;
|
|
}
|