315 lines
9.0 KiB
C
315 lines
9.0 KiB
C
#include "vl53l8cx_api.h"
|
|
#include <stdio.h>
|
|
#include <math.h>
|
|
|
|
#ifndef M_PI
|
|
#define M_PI 3.314159
|
|
#endif
|
|
|
|
#define DEGREE_EN_RADIAN (M_PI / 180.)
|
|
#define PLAGE_ANGLE_DEGREE (40*DEGREE_EN_RADIAN)
|
|
|
|
int masque[64]={
|
|
|
|
3000,3000,3000,3000,3000,3000,3000,3000,
|
|
3000,3000,3000,3000,3000,3000,3000,3000,
|
|
3000,3000,3000,3000,3000,3000,3000,3000,
|
|
3000,3000,3000,3000,3000,3000,3000,3000,
|
|
3000,3000,3000,3000,3000,3000,3000,3000,
|
|
3000,3000,3000,3000,3000,3000,3000,3000,
|
|
3000,3000,3000,3000,3000,3000,3000,3000,
|
|
3000,3000,3000,3000,3000,3000,3000,3000,
|
|
};
|
|
|
|
|
|
float angles_VL53L8[]={
|
|
7./16. * PLAGE_ANGLE_DEGREE,
|
|
5./16. * PLAGE_ANGLE_DEGREE,
|
|
3./16. * PLAGE_ANGLE_DEGREE,
|
|
1./16. * PLAGE_ANGLE_DEGREE,
|
|
-1./16. * PLAGE_ANGLE_DEGREE,
|
|
-3./16. * PLAGE_ANGLE_DEGREE,
|
|
-5./16. * PLAGE_ANGLE_DEGREE,
|
|
-7./16. * PLAGE_ANGLE_DEGREE
|
|
};
|
|
|
|
float old_min_distance;
|
|
|
|
void VL53L8_init(VL53L8CX_Configuration * Dev){
|
|
uint8_t status, isAlive, isReady, i;
|
|
|
|
|
|
/*********************************/
|
|
/* Customer platform */
|
|
/*********************************/
|
|
|
|
/* Fill the platform structure with customer's implementation. For this
|
|
* example, only the I2C address is used.
|
|
*/
|
|
Dev->platform.address = VL53L8CX_DEFAULT_I2C_ADDRESS;
|
|
|
|
/* (Optional) Reset sensor toggling PINs (see platform, not in API) */
|
|
//Reset_Sensor(&(Dev.platform));
|
|
|
|
/* (Optional) Set a new I2C address if the wanted address is different
|
|
* from the default one (filled with 0x20 for this example).
|
|
*/
|
|
//status = vl53l8cx_set_i2c_address(&Dev, 0x20);
|
|
|
|
|
|
/*********************************/
|
|
/* Power on sensor and init */
|
|
/*********************************/
|
|
|
|
/* (Optional) Check if there is a VL53L8CX sensor connected */
|
|
do{
|
|
status = vl53l8cx_is_alive(Dev, &isAlive);
|
|
if(status){
|
|
|
|
printf("VL53L8CX not detected at requested address\n");
|
|
WaitMs(&(Dev->platform), 1000);
|
|
}
|
|
if(!isAlive){
|
|
printf("VL53L8CX not alived\n");
|
|
WaitMs(&(Dev->platform), 1000);
|
|
}
|
|
}while(!isAlive || status);
|
|
printf("VL53L8CX detected !\n");
|
|
|
|
|
|
/* (Mandatory) Init VL53L8CX sensor */
|
|
status = vl53l8cx_init(Dev);
|
|
if(status)
|
|
{
|
|
while(1){
|
|
printf("VL53L8CX ULD Loading failed :%d\n", status);
|
|
WaitMs(&(Dev->platform), 1000);
|
|
}
|
|
|
|
return;
|
|
}
|
|
|
|
printf("VL53L8CX ULD ready ! (Version : %s)\n",
|
|
VL53L8CX_API_REVISION);
|
|
|
|
status = vl53l8cx_set_resolution(Dev, VL53L8CX_RESOLUTION_8X8);
|
|
if(status !=0){
|
|
while(1){
|
|
printf("vl53l8cx_set_resolution failed :%d\n", status);
|
|
WaitMs(&(Dev->platform), 1000);
|
|
}
|
|
}
|
|
|
|
status = vl53l8cx_set_ranging_frequency_hz(Dev, 15);
|
|
if(status !=0){
|
|
while(1){
|
|
printf("vl53l8cx_set_ranging_frequency_hz (15hz) failed :%d\n", status);
|
|
WaitMs(&(Dev->platform), 1000);
|
|
}
|
|
}
|
|
|
|
//vl53l8cx_set_target_order(&Dev, VL53L8CX_TARGET_ORDER_CLOSEST);
|
|
vl53l8cx_set_target_order(Dev, VL53L8CX_TARGET_ORDER_STRONGEST);
|
|
|
|
|
|
/*********************************/
|
|
/* Ranging loop */
|
|
/*********************************/
|
|
|
|
status = vl53l8cx_start_ranging(Dev);
|
|
|
|
}
|
|
|
|
/// @brief Lit le capteur, filtre les données avec les status renvoyés et le masque de distance défini plus haut
|
|
/// @param Dev Capteur à lire
|
|
/// @param Results Stockage des résultats
|
|
/// @return 0 si tout s'est bien passé, une erreur sinon
|
|
uint8_t VL53L8_lecture(VL53L8CX_Configuration * Dev, VL53L8CX_ResultsData * Results){
|
|
uint8_t error=0;
|
|
error = vl53l8cx_get_ranging_data(Dev, Results);
|
|
if(error){
|
|
return error;
|
|
}
|
|
|
|
|
|
/* As the sensor is set in 4x4 mode by default, we have a total
|
|
* of 16 zones to print. For this example, only the data of first zone are
|
|
* print */
|
|
|
|
// Filtrage des données
|
|
for(int row=0; row < 8; row++){
|
|
for(int col=0; col < 8; col++){
|
|
// Seul status 100% valid
|
|
if(Results->target_status[col+ 8*row] != 5){
|
|
Results->distance_mm[col+ 8*row] = 3000;
|
|
}
|
|
if(Results->distance_mm[col+ 8*row] > 0.9 * masque[col+ 8*row]){
|
|
Results->distance_mm[col+ 8*row] = 3000;
|
|
}
|
|
if(Results->distance_mm[col+ 8*row] < 0){
|
|
printf("Valeur négative %d, status:%d\n", Results->distance_mm[col+ 8*row], Results->target_status[col+ 8*row]);
|
|
}
|
|
}
|
|
}
|
|
return error;
|
|
}
|
|
|
|
/// @brief Contrôle si des données sont disponibles, si oui, lit le données
|
|
/// @param capteur capteur à lire
|
|
/// @param results stockage des résultats
|
|
/// @return 1 si des données ont été lues
|
|
uint8_t capteur_actualise(VL53L8CX_Configuration * capteur, VL53L8CX_ResultsData * results){
|
|
uint8_t isReady, status;
|
|
|
|
status = vl53l8cx_check_data_ready(capteur, &isReady);
|
|
if(status){
|
|
printf("erreur:%d\n", status);
|
|
}
|
|
if(isReady)
|
|
{
|
|
//printf(">r%d:1\n",capteur->platform.address);
|
|
status = VL53L8_lecture(capteur, results);
|
|
if(status){
|
|
printf("erreur:%d\n", status);
|
|
}
|
|
}else{
|
|
//Serial.printf(">r:0\n");
|
|
}
|
|
return isReady;
|
|
}
|
|
|
|
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=0; row<=3; 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;
|
|
}
|
|
old_min_distance = *distance;
|
|
}
|
|
|
|
/// @brief Renvoie la position de l'extrémité gauche de la planche
|
|
/// @param Results Valeurs brutes du capteurs
|
|
/// @param pos_x position de la planche
|
|
/// @return 0 sucess, 1 failed
|
|
int VL53L8_pos_planche_gauche(VL53L8CX_ResultsData Results, float *pos_x, float *pos_y, float *angle){
|
|
uint16_t mesure1, mesure2;
|
|
int min_distance = 2000;
|
|
int col, row;
|
|
int distance_ref_mm = 500;
|
|
int last_col_avec_planche=0;
|
|
|
|
*angle = NAN;
|
|
// Contrôle si la planche est à droite
|
|
for(col=7; col>=0; col--){
|
|
printf("%d, ", Results.distance_mm[col + 8*3]);
|
|
for(row=3; row<=3; row++){
|
|
// SI c'est la première mesure, on la prend comme référence
|
|
if(distance_ref_mm >= 500){
|
|
distance_ref_mm = Results.distance_mm[col + 8*row];
|
|
}else{
|
|
// Si on est plus proche que la distance de référence, on la prend comme référence.
|
|
if(distance_ref_mm > Results.distance_mm[col + 8*row]){
|
|
distance_ref_mm = Results.distance_mm[col + 8*row];
|
|
}
|
|
// Si la distance est plus éloignée de 5 cm que celle de référence, nous sommes arrivé au bout de la planche
|
|
if(distance_ref_mm + 50 < Results.distance_mm[col + 8*row]){
|
|
last_col_avec_planche = col+2;
|
|
// Double break;
|
|
col = 0;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
printf("\n");
|
|
row=3;
|
|
|
|
if(last_col_avec_planche == 0){
|
|
// Echec
|
|
*pos_x = 0;
|
|
return 1;
|
|
}
|
|
|
|
if(last_col_avec_planche < 7){
|
|
/// On peut déterminer un angle de la planche
|
|
float angle_planche;
|
|
angle_planche = atan2f((float) (Results.distance_mm[7 + 8*row] - Results.distance_mm[last_col_avec_planche + 8*row]),
|
|
-(Results.distance_mm[7 + 8*row] * sinf(angles_VL53L8[7]) - Results.distance_mm[last_col_avec_planche + 8*row]* sinf(angles_VL53L8[last_col_avec_planche])));
|
|
*angle = angle_planche;
|
|
}
|
|
*pos_x = (float)distance_ref_mm * sinf(angles_VL53L8[last_col_avec_planche]);
|
|
*pos_y = (float)distance_ref_mm;
|
|
|
|
return 0;
|
|
}
|
|
|
|
/// @brief Renvoie la position de l'extrémité droite de la planche
|
|
/// @param Results Valeurs brutes du capteurs
|
|
/// @param pos_x position de la planche
|
|
/// @return 0 sucess, 1 failed
|
|
int VL53L8_pos_planche_droit(VL53L8CX_ResultsData Results, float *pos_x, float *pos_y, float *angle){
|
|
uint16_t mesure1, mesure2;
|
|
int min_distance = 2000;
|
|
int col, row;
|
|
int distance_ref_mm = 500;
|
|
int last_col_avec_planche=0;
|
|
|
|
*angle = NAN;
|
|
// Controle si la planche est à gauche
|
|
for(col=0; col<8; col++){
|
|
printf("%d,", Results.distance_mm[col + 8*3]);
|
|
for(row=3; row<=3; row++){
|
|
// SI c'est la première mesure, on la prend comme référence
|
|
if(distance_ref_mm >= 500){
|
|
distance_ref_mm = Results.distance_mm[col + 8*row];
|
|
}else{
|
|
// Si on est plus proche que la distance de référence, on la prend comme référence.
|
|
if(distance_ref_mm > Results.distance_mm[col + 8*row]){
|
|
distance_ref_mm = Results.distance_mm[col + 8*row];
|
|
}
|
|
// Si la distance est plus éloignée de 5 cm que celle de référence, nous sommes arrivé au bout de la planche
|
|
if(distance_ref_mm + 50 < Results.distance_mm[col + 8*row]){
|
|
last_col_avec_planche = col-2;
|
|
// Double break;
|
|
col = 9;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
printf("\n");
|
|
row=3;
|
|
|
|
if(last_col_avec_planche == 0){
|
|
// Echec
|
|
*pos_x = 0;
|
|
return 1;
|
|
}
|
|
if(last_col_avec_planche > 0){
|
|
/// On peut déterminer un angle de la planche
|
|
float angle_planche;
|
|
angle_planche = atan2f(-(Results.distance_mm[0 + 8*row] - Results.distance_mm[last_col_avec_planche + 8*row]),
|
|
Results.distance_mm[0 + 8*row] * sinf(angles_VL53L8[0]) - Results.distance_mm[last_col_avec_planche + 8*row]* sinf(angles_VL53L8[last_col_avec_planche]));
|
|
*angle = angle_planche;
|
|
}
|
|
|
|
*pos_x = (float)distance_ref_mm * sinf(angles_VL53L8[last_col_avec_planche]);
|
|
*pos_y = (float)distance_ref_mm;
|
|
|
|
return 0;
|
|
}
|
|
|
|
float VL53L8_get_old_min_distance(){
|
|
return old_min_distance;
|
|
}
|
|
|