278 lines
6.6 KiB
C
278 lines
6.6 KiB
C
/*****
|
|
* Copyright (c) 2023 - Poivron Robotique
|
|
*
|
|
* SPDX-License-Identifier: BSD-3-Clause
|
|
*/
|
|
#include "pico/stdlib.h"
|
|
#include "pico/multicore.h"
|
|
#include "hardware/adc.h"
|
|
#include "hardware/spi.h"
|
|
#include "i2c_maitre.h"
|
|
#include "Temps.h"
|
|
#include "VL53L8_2024.h"
|
|
#include "vl53l8cx_api.h"
|
|
#include <stdio.h>
|
|
#include <math.h>
|
|
|
|
#define LED1PIN 20
|
|
#define TIRETTE_PIN 6
|
|
#define COULEUR_PIN 4
|
|
|
|
#define COULEUR_BLEU 1
|
|
#define COULEUR_JAUNE 0
|
|
|
|
#define OFFSET_CAPTEUR_GAUCHE_X_MM (-170)
|
|
#define OFFSET_CAPTEUR_DROIT_Y_MM (170)
|
|
|
|
// XIAO RP2040
|
|
#define SCK 2
|
|
#define MISO 4
|
|
#define MOSI 3
|
|
#define D0 26
|
|
#define D1 27
|
|
#define D2 28
|
|
#define D3 29
|
|
|
|
|
|
void affichage(void);
|
|
void gestion_affichage(void);
|
|
void tension_batterie_init(void);
|
|
uint16_t tension_batterie_lire(void);
|
|
|
|
|
|
int get_tirette(int);
|
|
int get_couleur(void);
|
|
void configure_trajet(int identifiant, int couleur);
|
|
|
|
void gestion_VL53L8CX(void);
|
|
|
|
const uint32_t step_ms=1;
|
|
float distance1_mm=0, distance2_mm=0;
|
|
|
|
// DEBUG
|
|
extern float abscisse;
|
|
extern struct point_xyo_t point;
|
|
float vitesse;
|
|
VL53L8CX_Configuration gauche, droit;
|
|
|
|
uint8_t capteur_init(VL53L8CX_Configuration * capteur);
|
|
|
|
void main(void)
|
|
{
|
|
int ledpower = 500;
|
|
uint8_t tampon[10];
|
|
uint8_t temp, status;
|
|
VL53L8CX_ResultsData Results;
|
|
bool fin_match = false;
|
|
|
|
|
|
|
|
stdio_init_all();
|
|
stdio_set_translate_crlf(&stdio_usb, false);
|
|
Temps_init();
|
|
//tension_batterie_init();
|
|
spi_init(spi0, 2000000);
|
|
|
|
|
|
|
|
uint32_t temps_ms = Temps_get_temps_ms();
|
|
uint32_t temps_depart_ms;
|
|
float vitesse_mm_s=100;
|
|
|
|
|
|
gpio_init(LED1PIN);
|
|
gpio_set_dir(LED1PIN, GPIO_OUT );
|
|
gpio_put(LED1PIN, 1);
|
|
|
|
// Initialisation de la liaison SPI0
|
|
gpio_set_function(2, GPIO_FUNC_SPI);
|
|
gpio_set_function(3, GPIO_FUNC_SPI);
|
|
gpio_set_function(4, GPIO_FUNC_SPI);
|
|
|
|
// Initialisation de la liaison SPI1
|
|
gpio_set_function(26, GPIO_FUNC_SPI);
|
|
gpio_set_function(27, GPIO_FUNC_SPI);
|
|
gpio_set_function(28, GPIO_FUNC_SPI);
|
|
|
|
gpio_set_function(16, GPIO_FUNC_NULL);
|
|
gpio_set_function(17, GPIO_FUNC_NULL);
|
|
gpio_set_function(18, GPIO_FUNC_NULL);
|
|
gpio_set_function(19, GPIO_FUNC_NULL);
|
|
|
|
gpio_init(1); // CS
|
|
gpio_set_dir(1, GPIO_OUT );
|
|
gpio_put(1, 1);
|
|
|
|
gpio_init(29); // CS
|
|
gpio_set_dir(29, GPIO_OUT );
|
|
gpio_put(29, 1);
|
|
|
|
|
|
gauche.platform.address = 1;
|
|
droit.platform.address = 29;
|
|
|
|
spi_set_format(spi0, 8, SPI_CPOL_1, SPI_CPHA_1, SPI_MSB_FIRST);
|
|
spi_init(spi0, 2000000);
|
|
|
|
spi_set_format(spi1, 8, SPI_CPOL_1, SPI_CPHA_1, SPI_MSB_FIRST);
|
|
spi_init(spi1, 2000000);
|
|
|
|
tampon[0] = 0x55;
|
|
tampon[1] = 0x55;
|
|
|
|
sleep_ms(5000);
|
|
printf("Demarrage...\n");
|
|
|
|
//multicore_launch_core1(gestion_affichage);
|
|
|
|
|
|
gestion_VL53L8CX();
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
uint8_t capteur_init(VL53L8CX_Configuration * capteur){
|
|
uint8_t status=0;
|
|
printf("debut init\n");
|
|
//pinMode(capteur->platform.address, OUTPUT);
|
|
|
|
status = vl53l8cx_init(capteur);
|
|
if(status != 0){
|
|
while(1){
|
|
printf("Status init = %d\n", status);
|
|
WaitMs(&(capteur->platform), 1000);
|
|
break;
|
|
}
|
|
}
|
|
sleep_ms(100);
|
|
|
|
status = vl53l8cx_set_resolution(capteur, VL53L8CX_RESOLUTION_8X8);
|
|
if(status !=0){
|
|
while(1){
|
|
printf("vl53l8cx_set_resolution failed :%d\n", status);
|
|
WaitMs(&(capteur->platform), 1000);
|
|
break;
|
|
}
|
|
}
|
|
|
|
status = vl53l8cx_set_ranging_frequency_hz(capteur, 15);
|
|
if(status !=0){
|
|
while(1){
|
|
printf("vl53l8cx_set_ranging_frequency_hz (15hz) failed :%d\n", status);
|
|
WaitMs(&(capteur->platform), 1000);
|
|
break;
|
|
}
|
|
}
|
|
|
|
//vl53l8cx_set_target_order(&Dev, VL53L8CX_TARGET_ORDER_CLOSEST);
|
|
vl53l8cx_set_target_order(capteur, VL53L8CX_TARGET_ORDER_STRONGEST);
|
|
|
|
status = vl53l8cx_start_ranging(capteur);
|
|
|
|
printf("Capteur : %d, fin init: %d\n", capteur->platform.address, status);
|
|
return status;
|
|
|
|
}
|
|
|
|
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 = vl53l8cx_get_ranging_data(capteur, results);
|
|
if(status){
|
|
printf("erreur:%d\n", status);
|
|
}
|
|
}else{
|
|
//Serial.printf(">r:0\n");
|
|
}
|
|
return isReady;
|
|
}
|
|
|
|
void capteurs_affiche_donnees(VL53L8CX_ResultsData * results_gauche, VL53L8CX_ResultsData * results_droit){
|
|
uint8_t row, col;
|
|
for(col=0; col<8; col++){
|
|
for(row=0; row<8; row++){
|
|
printf("%4d ", results_gauche->distance_mm[col*8 + row]);
|
|
}
|
|
printf(" - ");
|
|
for(row=0; row<8; row++){
|
|
printf("%4d ", results_droit->distance_mm[col*8 + row]);
|
|
}
|
|
|
|
printf("\n");
|
|
}
|
|
printf("\n");
|
|
}
|
|
|
|
/// @brief Obtient la distance de l'obstacle le plus proche.
|
|
/// @param
|
|
|
|
float gauche_planche_pos_x, gauche_planche_pos_y;
|
|
float droit_planche_pos_x, droit_planche_pos_y;
|
|
|
|
float planche_centre_x, planche_centre_y, planche_angle_rad;
|
|
float taille_planche;
|
|
|
|
void gestion_VL53L8CX(void){
|
|
VL53L8CX_ResultsData results_gauche, results_droit;
|
|
float distance_obstacle;
|
|
int echec;
|
|
capteur_init(&gauche);
|
|
capteur_init(&droit);
|
|
sleep_ms(100);
|
|
capteur_actualise( &gauche, &results_gauche); // une première lecture
|
|
capteur_actualise( &droit, &results_droit); // une première lecture
|
|
uint8_t status, isReady;
|
|
while(1){
|
|
isReady = 0;
|
|
isReady |= capteur_actualise( &gauche, &results_gauche);
|
|
isReady |= capteur_actualise( &droit, &results_droit);
|
|
if(isReady){
|
|
//capteurs_affiche_donnees(&results_gauche, &results_droit);
|
|
//VL53L8_lecture( &gauche, &Results);
|
|
echec = 0;
|
|
echec |= VL53L8_pos_planche_gauche(results_gauche, &gauche_planche_pos_x, &gauche_planche_pos_y);
|
|
echec |= VL53L8_pos_planche_droit(results_droit, &droit_planche_pos_x, &droit_planche_pos_y);
|
|
|
|
droit_planche_pos_x = -droit_planche_pos_x + OFFSET_CAPTEUR_DROIT_Y_MM;
|
|
gauche_planche_pos_x = -gauche_planche_pos_x + OFFSET_CAPTEUR_GAUCHE_X_MM;
|
|
|
|
planche_centre_x = (droit_planche_pos_x + gauche_planche_pos_x)/2;
|
|
planche_centre_y = (droit_planche_pos_y + gauche_planche_pos_y)/2;
|
|
planche_angle_rad = atan2f(droit_planche_pos_y - gauche_planche_pos_y, droit_planche_pos_x - gauche_planche_pos_x);
|
|
}
|
|
affichage();
|
|
printf("\n");
|
|
sleep_ms(150);
|
|
}
|
|
}
|
|
|
|
|
|
extern float delta_x_mm, delta_y_mm, delta_orientation_radian;
|
|
|
|
void gestion_affichage(void){
|
|
while(1){
|
|
affichage();
|
|
}
|
|
}
|
|
|
|
void affichage(void){
|
|
/*
|
|
printf(">planche_g_pos_x:%f\n",gauche_planche_pos_x);
|
|
printf(">planche_g_pos_y:%f\n",gauche_planche_pos_y);
|
|
printf(">planche_d_pos_x:%f\n",droit_planche_pos_x);
|
|
printf(">planche_d_pos_y:%f\n",droit_planche_pos_y);*/
|
|
printf(">planche_centre_x:%f\n",planche_centre_x);
|
|
printf(">planche_centre_y:%f\n",planche_centre_y);
|
|
printf(">planche_angle:%f\n",planche_angle_rad / M_PI * 180);
|
|
|
|
|
|
} |