339 lines
9.9 KiB
C
339 lines
9.9 KiB
C
#include "pico/stdlib.h"
|
|
#include <stdio.h>
|
|
#include "VL53L1X_api.h"
|
|
#include "VL53L1X_calibration.h"
|
|
#include "VL53L1X_Fonctions.h"
|
|
#include "SelectionCapteur.h"
|
|
#include "ws2812.h"
|
|
|
|
#define DISTANCE_TROP_LOIN_CM 200 /* Distance de saturation */
|
|
#define DISTANCE_TRES_LOIN_CM 120 /* Seuil min. pour la couleur bleu*/
|
|
#define DISTANCE_LOIN_CM 80 /* Seuil min. pour la couleur verte*/
|
|
#define DISTANCE_PROCHE_CM 14 /* Seuil entre violet et jaune*/
|
|
|
|
#define NB_CAPTEURS 12
|
|
|
|
// Stock les valeurs lues des capteurs
|
|
uint8_t distance_capteur_cm[12];
|
|
|
|
uint8_t statu_capteurs[13];
|
|
enum {
|
|
MODE_DISTANCE,
|
|
MODE_MANUEL
|
|
} mode_led[NB_CAPTEURS];
|
|
|
|
void reset_affichage_led(void);
|
|
|
|
void initialise_adresses(void){
|
|
const uint8_t tmp_i2c_adresse = 0x28;
|
|
const uint8_t default_i2c_adresse = 0x29;
|
|
uint8_t adresse = default_i2c_adresse;
|
|
|
|
|
|
|
|
// On change l'adresse de tous les capteurs
|
|
Selection_capteur_deselect();
|
|
change_address(&adresse, tmp_i2c_adresse);
|
|
|
|
reset_affichage_led();
|
|
|
|
// Pour chaque capteur
|
|
for(uint capteur=1; capteur<=NB_CAPTEURS; capteur++){
|
|
// reset du capteur
|
|
Selection_capteur_select(capteur);
|
|
sleep_ms(1);
|
|
Selection_capteur_deselect();
|
|
sleep_ms(1);
|
|
uint8_t VL53L1X_device = 0x29;
|
|
|
|
if(change_address(&VL53L1X_device, 0x30 + capteur)){
|
|
printf("Erreur change adresse : %x => %x, capteur : %d\n", VL53L1X_device, 0x30 + capteur, capteur);
|
|
ws2812_set_buffer_rgb(0x4, 0, 0, capteur-1);
|
|
statu_capteurs[capteur]=0;
|
|
}else{
|
|
if(VL53L1X_SensorInit(VL53L1X_device)){
|
|
// bad init
|
|
ws2812_set_buffer_rgb(0x4, 0, 0, capteur-1);
|
|
statu_capteurs[capteur]=0;
|
|
}else{
|
|
// good init
|
|
statu_capteurs[capteur]=1;
|
|
int status;
|
|
status = VL53L1X_SetDistanceMode (VL53L1X_device, 1); // Short mode
|
|
status |= VL53L1X_SetInterMeasurementInMs(VL53L1X_device, 200);
|
|
status |= VL53L1X_SetTimingBudgetInMs(VL53L1X_device, 200);
|
|
if(status){
|
|
printf("Custom config KO, error %d\n", status);
|
|
ws2812_set_buffer_rgb(0x4, 0, 0, capteur-1);
|
|
}else{
|
|
printf("Custom config OK\n");
|
|
}
|
|
|
|
status=VL53L1X_StartRanging(VL53L1X_device);
|
|
if(!status){
|
|
ws2812_set_buffer_rgb(0, 0x4, 0, capteur-1);
|
|
}else{
|
|
ws2812_set_buffer_rgb(0x2, 0x2, 0, capteur-1);
|
|
mode_led[capteur-1]=MODE_DISTANCE;
|
|
}
|
|
|
|
|
|
}
|
|
}
|
|
}
|
|
ws2812_affiche_buffer();
|
|
|
|
ws2812_affiche_buffer();
|
|
|
|
}
|
|
|
|
int change_address(uint8_t *device, uint8_t new_i2c_7bits_address){
|
|
int status;
|
|
status = VL53L1X_SetI2CAddress(*device, new_i2c_7bits_address << 1);
|
|
if(status){
|
|
//printf("VL53L1X_SetI2CAddress, Error :%d\n", status);
|
|
}else{
|
|
*device=new_i2c_7bits_address;
|
|
}
|
|
return status;
|
|
}
|
|
|
|
/// @brief Interroge les capteurs les uns après les autres.
|
|
/// Nécessite que les capteurs soient en mode lecture continue avec VL53L1X_StartRanging(device)
|
|
/// @return 1
|
|
int continuous_multiple_reading(){
|
|
for(uint8_t device=0x31; device<0x31+12; device++){
|
|
if(statu_capteurs[device-0x31+1]==0){
|
|
continue;
|
|
}
|
|
int status;
|
|
uint8_t data_ready = 0;
|
|
uint16_t distance_mm;
|
|
while(!data_ready){
|
|
status=VL53L1X_CheckForDataReady(device, &data_ready);
|
|
if(status){
|
|
printf("CheckForDataReady KO, error %d, capteur:%x\n", status, device);
|
|
}
|
|
}
|
|
|
|
status=VL53L1X_GetDistance(device, &distance_mm);
|
|
if(status){
|
|
printf("GetDistance KO, error %d, capteur:%x\n", status, device);
|
|
return 0;
|
|
}else{
|
|
printf(">distance%x:%d\n", device, distance_mm);
|
|
if(distance_mm < DISTANCE_TROP_LOIN_CM * 10){
|
|
distance_capteur_cm[device-0x31] = distance_mm / 10;
|
|
}else{
|
|
distance_capteur_cm[device-0x31] = DISTANCE_TROP_LOIN_CM;
|
|
}
|
|
}
|
|
|
|
status=VL53L1X_ClearInterrupt(device);
|
|
if(status){
|
|
printf("ClearInterrupt KO, error %d, capteur:%x\n", status, device);
|
|
return 0;
|
|
}
|
|
|
|
int lettre = getchar_timeout_us(0);
|
|
if(lettre != PICO_ERROR_TIMEOUT && lettre != 0){
|
|
//return 0;
|
|
}
|
|
}
|
|
affiche_distance_sur_led();
|
|
return 1;
|
|
}
|
|
|
|
int continuous_special_reading(){
|
|
for(uint8_t device=0x32; device<0x31+12; device+=6){
|
|
if(statu_capteurs[device-0x31+1]==0){
|
|
continue;
|
|
}
|
|
int status;
|
|
uint8_t data_ready = 0;
|
|
uint16_t distance_mm;
|
|
while(!data_ready){
|
|
status=VL53L1X_CheckForDataReady(device, &data_ready);
|
|
if(status){
|
|
printf("CheckForDataReady KO, error %d, capteur:%x\n", status, device);
|
|
}
|
|
}
|
|
|
|
status=VL53L1X_GetDistance(device, &distance_mm);
|
|
if(status){
|
|
printf("GetDistance KO, error %d, capteur:%x\n", status, device);
|
|
return 0;
|
|
}else{
|
|
printf(">distance%x:%d\n", device, distance_mm);
|
|
if(distance_mm < DISTANCE_TROP_LOIN_CM * 10){
|
|
distance_capteur_cm[device-0x31] = distance_mm / 10;
|
|
}else{
|
|
distance_capteur_cm[device-0x31] = DISTANCE_TROP_LOIN_CM;
|
|
}
|
|
}
|
|
|
|
status=VL53L1X_ClearInterrupt(device);
|
|
if(status){
|
|
printf("ClearInterrupt KO, error %d, capteur:%x\n", status, device);
|
|
return 0;
|
|
}
|
|
|
|
int lettre = getchar_timeout_us(0);
|
|
if(lettre != PICO_ERROR_TIMEOUT && lettre != 0){
|
|
//return 0;
|
|
}
|
|
}
|
|
affiche_distance_sur_led();
|
|
return 1;
|
|
}
|
|
|
|
|
|
void affiche_distance_sur_led(){
|
|
uint8_t distance_cm;
|
|
uint32_t couleur;
|
|
for(uint8_t capteur=0; capteur<12; capteur++){
|
|
if(mode_led[capteur] == MODE_DISTANCE){
|
|
distance_cm = distance_capteur_cm[capteur];
|
|
if(distance_cm == 0 ||distance_cm > DISTANCE_TRES_LOIN_CM){
|
|
ws2812_set_buffer_rgb(COULEUR_TRES_LOIN, capteur);
|
|
}else if(distance_cm > DISTANCE_LOIN_CM){
|
|
ws2812_set_buffer_rgb(COULEUR_LOIN, capteur);
|
|
}else if(distance_cm > DISTANCE_PROCHE_CM){
|
|
ws2812_set_buffer_rgb(COULEUR_PROCHE, capteur);
|
|
}else{
|
|
ws2812_set_buffer_rgb(COULEUR_TROP_PROCHE, capteur);
|
|
}
|
|
}
|
|
}
|
|
ws2812_affiche_buffer();
|
|
}
|
|
|
|
void affiche_couleur_sur_led(uint8_t couleur_8bits, uint8_t led){
|
|
mode_led[led] = MODE_MANUEL;
|
|
ws2812_set_buffer_8bits(couleur_8bits, led);
|
|
}
|
|
|
|
/// @brief Remet toutes les LEDs en mode d'affichage de la distance
|
|
void reset_affichage_led(){
|
|
for(uint8_t capteur=0; capteur<12; capteur++){
|
|
mode_led[capteur] = MODE_DISTANCE;
|
|
}
|
|
}
|
|
|
|
int calibration(uint8_t device){
|
|
uint16_t offset;
|
|
uint16_t x_talk;
|
|
int status;
|
|
uint8_t boot_state=0;
|
|
printf("Calibration...\n");
|
|
while(!boot_state){
|
|
VL53L1X_BootState(device, &boot_state);
|
|
}
|
|
printf("Sensor boot ok\n");
|
|
|
|
status=VL53L1X_SensorInit(device);
|
|
if(status){
|
|
printf("Sensor Init KO, error %d\n", status);
|
|
}else{
|
|
printf("Sensor Init OK\n");
|
|
}
|
|
|
|
status = VL53L1X_CalibrateOffset(device, 140, &offset);
|
|
if(status != 0){
|
|
printf("Error while calibrating : %d\n",status);
|
|
}else{
|
|
printf("Offset : %d\n", offset);
|
|
|
|
}
|
|
/*
|
|
// Renvoie x_talk = 0 si la calibration se passe bien car
|
|
// nous n'avons pas de vitre de protection devant le capteur
|
|
status = VL53L1X_CalibrateXtalk(device, 1000, &x_talk);
|
|
if(status != 0){
|
|
printf("Error while calibrating : %d\n",status);
|
|
}else{
|
|
printf("xTalk : %d\n", x_talk);
|
|
|
|
}
|
|
*/
|
|
|
|
return 0;
|
|
}
|
|
|
|
int continuous_reading(uint8_t device){
|
|
int status;
|
|
uint8_t data_ready, boot_state=0;
|
|
uint16_t distance;
|
|
|
|
printf("Reading distance...\nSend any character to quit.");
|
|
|
|
while(!boot_state){
|
|
VL53L1X_BootState(device, &boot_state);
|
|
}
|
|
printf("Sensor boot ok\n");
|
|
|
|
status=VL53L1X_SensorInit(device);
|
|
if(status){
|
|
printf("Sensor Init KO, error %d\n", status);
|
|
return 0;
|
|
}else{
|
|
printf("Sensor Init OK\n");
|
|
}
|
|
|
|
|
|
// Custom configuration
|
|
status = VL53L1X_SetDistanceMode (device, 1); // Short mode
|
|
status |= VL53L1X_SetInterMeasurementInMs(device, 200);
|
|
status |= VL53L1X_SetTimingBudgetInMs(device, 200);
|
|
if(status){
|
|
printf("Custom config KO, error %d\n", status);
|
|
return 0;
|
|
}else{
|
|
printf("Custom config OK\n");
|
|
}
|
|
|
|
status=VL53L1X_StartRanging(device);
|
|
if(status){
|
|
printf("Start ranging KO, error %d\n", status);
|
|
return 0;
|
|
}else{
|
|
printf("Start ranging OK\n");
|
|
}
|
|
|
|
while(1){
|
|
// Reading data
|
|
data_ready = 0;
|
|
while(!data_ready){
|
|
status=VL53L1X_CheckForDataReady(device, &data_ready);
|
|
if(status){
|
|
printf("CheckForDataReady KO, error %d\n", status);
|
|
return 0;
|
|
}else{
|
|
//printf("CheckForDataReady OK\n");
|
|
}
|
|
}
|
|
|
|
status=VL53L1X_GetDistance(device, &distance);
|
|
if(status){
|
|
printf("GetDistance KO, error %d\n", status);
|
|
return 0;
|
|
}else{
|
|
//printf("GetDistance OK, distance %u mm\n", distance);
|
|
printf(">distance:%d\n", distance);
|
|
}
|
|
|
|
status=VL53L1X_ClearInterrupt(device);
|
|
if(status){
|
|
printf("ClearInterrupt KO, error %d\n", status);
|
|
return 0;
|
|
}else{
|
|
//printf("ClearInterrupt OK\n");
|
|
}
|
|
|
|
int lettre = getchar_timeout_us(0);
|
|
if(lettre != PICO_ERROR_TIMEOUT && lettre != 0){
|
|
return 0;
|
|
}
|
|
}
|
|
return 0;
|
|
} |