66 lines
2.4 KiB
C
66 lines
2.4 KiB
C
|
#include "i2c_annexe.h"
|
||
|
|
#include "Localisation.h"
|
||
|
|
#include "math.h"
|
||
|
|
|
||
|
|
#define NB_CAPTEURS 12
|
||
|
|
#define DISTANCE_OBSTACLE_IGNORE 200
|
||
|
|
#define DISTANCE_CAPTEUR_CENTRE_ROBOT 40
|
||
|
|
|
||
|
|
uint8_t distance_capteur_cm[NB_CAPTEURS];
|
||
|
|
|
||
|
|
struct capteur_VL53L1X_t{
|
||
|
|
uint8_t distance_cm;
|
||
|
|
double angle_ref_robot;
|
||
|
|
double angle_ref_terrain;
|
||
|
|
}capteurs_VL53L1X[NB_CAPTEURS];
|
||
|
|
|
||
|
|
void actualise_VL53L1X(struct capteur_VL53L1X_t * capteur_VL53L1X, uint8_t distance_capteur_cm);
|
||
|
|
|
||
|
|
void Balise_VL53L1X_gestion(){
|
||
|
|
i2c_annexe_get_distances(distance_capteur_cm);
|
||
|
|
for(uint8_t capteur=0; capteur<NB_CAPTEURS; capteur++){
|
||
|
|
actualise_VL53L1X(&(capteurs_VL53L1X[capteur]), distance_capteur_cm[capteur]);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void actualise_VL53L1X(struct capteur_VL53L1X_t * capteur_VL53L1X, uint8_t distance_capteur_cm){
|
||
|
|
// Actualisation de l'angle du capteur
|
||
|
|
capteur_VL53L1X->angle_ref_terrain = capteur_VL53L1X->angle_ref_robot + Localisation_get().angle_radian;
|
||
|
|
// Maintien de l'angle entre -PI et PI
|
||
|
|
while(capteur_VL53L1X->angle_ref_terrain > 2* M_PI){
|
||
|
|
capteur_VL53L1X->angle_ref_terrain -= 2* M_PI;
|
||
|
|
}
|
||
|
|
while(capteur_VL53L1X->angle_ref_terrain < 2* -M_PI){
|
||
|
|
capteur_VL53L1X->angle_ref_terrain += 2* M_PI;
|
||
|
|
}
|
||
|
|
capteur_VL53L1X->distance_cm = distance_capteur_cm + DISTANCE_CAPTEUR_CENTRE_ROBOT;
|
||
|
|
}
|
||
|
|
|
||
|
|
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 = DISTANCE_OBSTACLE_IGNORE;
|
||
|
|
for(uint8_t capteur=0; capteur<NB_CAPTEURS; capteur++){
|
||
|
|
if(min_distance> capteurs_VL53L1X[capteur].distance_cm){
|
||
|
|
min_distance = capteurs_VL53L1X[capteur].distance_cm;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return min_distance;
|
||
|
|
}
|
||
|
|
|
||
|
|
uint8_t Balise_VL53L1X_get_capteur_cm(uint8_t capteur){
|
||
|
|
return capteurs_VL53L1X[capteur].distance_cm;
|
||
|
|
}
|