390 lines
10 KiB
C
390 lines
10 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 "hardware/i2c.h"
|
|
#include "i2c_slave.h"
|
|
#include "Temps.h"
|
|
#include "VL53L8_2024.h"
|
|
#include "vl53l8cx_api.h"
|
|
#include "ws2812.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_X_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
|
|
#define SDA 6
|
|
#define SCL 7
|
|
|
|
#define I2C_SLAVE_SDA_PIN SDA
|
|
#define I2C_SLAVE_SCL_PIN SCL
|
|
#define I2C_SLAVE_ADDRESS 0x19
|
|
|
|
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);
|
|
|
|
|
|
// The slave implements a 256 byte memory. To write a series of bytes, the master first
|
|
// writes the memory address, followed by the data. The address is automatically incremented
|
|
// for each byte transferred, looping back to 0 upon reaching the end. Reading is done
|
|
// sequentially from the current memory address.
|
|
static struct
|
|
{
|
|
uint8_t mem[256];
|
|
uint8_t mem_address;
|
|
bool mem_address_written;
|
|
} context;
|
|
|
|
// Our handler is called from the I2C ISR, so it must complete quickly. Blocking calls /
|
|
// printing to stdio may interfere with interrupt handling.
|
|
static void i2c_slave_handler(i2c_inst_t *i2c, i2c_slave_event_t event) {
|
|
switch (event) {
|
|
case I2C_SLAVE_RECEIVE: // master has written some data
|
|
if (!context.mem_address_written) {
|
|
// writes always start with the memory address
|
|
context.mem_address = i2c_read_byte_raw(i2c);
|
|
context.mem_address_written = true;
|
|
} else {
|
|
// save into memory
|
|
context.mem[context.mem_address] = i2c_read_byte_raw(i2c);
|
|
context.mem_address++;
|
|
}
|
|
break;
|
|
case I2C_SLAVE_REQUEST: // master is requesting data
|
|
// load from memory
|
|
i2c_write_byte_raw(i2c, context.mem[context.mem_address]);
|
|
context.mem_address++;
|
|
break;
|
|
case I2C_SLAVE_FINISH: // master has signalled Stop / Restart
|
|
context.mem_address_written = false;
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
void i2c_envoi_32bits(int32_t value, char adresse){
|
|
context.mem[adresse] = value >> 24;
|
|
context.mem[adresse+1] = (value >> 16) & 0xFF;
|
|
context.mem[adresse+2] = (value >> 8) & 0xFF;
|
|
context.mem[adresse+3] = value & 0xFF;
|
|
}
|
|
|
|
static void i2c_setup_slave() {
|
|
gpio_init(I2C_SLAVE_SDA_PIN);
|
|
gpio_set_function(I2C_SLAVE_SDA_PIN, GPIO_FUNC_I2C);
|
|
gpio_pull_up(I2C_SLAVE_SDA_PIN);
|
|
|
|
gpio_init(I2C_SLAVE_SCL_PIN);
|
|
gpio_set_function(I2C_SLAVE_SCL_PIN, GPIO_FUNC_I2C);
|
|
gpio_pull_up(I2C_SLAVE_SCL_PIN);
|
|
|
|
i2c_slave_init(i2c1, I2C_SLAVE_ADDRESS, &i2c_slave_handler);
|
|
}
|
|
|
|
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);
|
|
|
|
ws2812_init();
|
|
|
|
|
|
|
|
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;
|
|
|
|
i2c_setup_slave();
|
|
|
|
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;
|
|
|
|
}
|
|
|
|
|
|
|
|
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, gauche_planche_angle;
|
|
float droit_planche_pos_x, droit_planche_pos_y, droit_planche_angle;
|
|
|
|
float planche_centre_x, planche_centre_y, planche_angle_rad;
|
|
float taille_planche;
|
|
int echec;
|
|
|
|
void gestion_VL53L8CX(void){
|
|
VL53L8CX_ResultsData results_gauche, results_droit;
|
|
int echec_gauche, echec_droit;
|
|
float distance_obstacle;
|
|
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_gauche = VL53L8_pos_planche_gauche(results_gauche, &gauche_planche_pos_x, &gauche_planche_pos_y, &gauche_planche_angle);
|
|
echec_droit = VL53L8_pos_planche_droit(results_droit, &droit_planche_pos_x, &droit_planche_pos_y, &droit_planche_angle);
|
|
printf("g:%d,d:%d\n", echec_gauche, echec_droit );
|
|
|
|
droit_planche_pos_x = -droit_planche_pos_x + OFFSET_CAPTEUR_DROIT_X_MM;
|
|
gauche_planche_pos_x = -gauche_planche_pos_x + OFFSET_CAPTEUR_GAUCHE_X_MM;
|
|
|
|
echec = (echec_gauche > 0) + (echec_droit > 0);
|
|
|
|
if(echec == 2){
|
|
if(echec_gauche == 2 && echec_droit == 3){
|
|
ws2812_set(0, 0, 0xF);
|
|
context.mem[0] = 3;
|
|
}else if(echec_gauche == 3 && echec_droit == 2){
|
|
ws2812_set(0, 0, 0xF);
|
|
context.mem[0] = 4;
|
|
}else{
|
|
// Aucun capteur valide
|
|
ws2812_set(0x0F,0,0);
|
|
context.mem[0] = 0;
|
|
}
|
|
}else if(echec == 1){
|
|
// Un seul capteur valide
|
|
if(echec_gauche == 0 && ! isnan(gauche_planche_angle)){
|
|
// capteur gauche permet de déterminer la position de la planche
|
|
ws2812_set(0x0F,0x8,0);
|
|
planche_centre_x = gauche_planche_pos_x + 200 * cos(gauche_planche_angle);
|
|
planche_centre_y = gauche_planche_pos_y + 200 * sin(gauche_planche_angle);
|
|
planche_angle_rad = gauche_planche_angle;
|
|
i2c_envoi_32bits(planche_centre_x, 1);
|
|
i2c_envoi_32bits(planche_centre_y, 5);
|
|
i2c_envoi_32bits((int)(planche_angle_rad * 1000), 9);
|
|
context.mem[0] = 1;
|
|
|
|
}else if(echec_droit == 0 && ! isnan(droit_planche_angle)){
|
|
// capteur droit permet de déterminer la position de la planche
|
|
ws2812_set(0x0F,0x8,0);
|
|
planche_centre_x = droit_planche_pos_x - 200 * cos(droit_planche_angle);
|
|
planche_centre_y = droit_planche_pos_y - 200 * sin(droit_planche_angle);
|
|
planche_angle_rad = droit_planche_angle;
|
|
i2c_envoi_32bits(planche_centre_x, 1);
|
|
i2c_envoi_32bits(planche_centre_y, 5);
|
|
i2c_envoi_32bits((int)(planche_angle_rad * 1000), 9);
|
|
context.mem[0] = 1;
|
|
|
|
}else{
|
|
// On a un bout de la planche mais pas d'angle, c'est un echec
|
|
echec = 2;
|
|
ws2812_set(0x0F,0,0);
|
|
context.mem[0] = 0;
|
|
}
|
|
|
|
}else{
|
|
// 2 capteurs valides
|
|
ws2812_set(0,0x0F,0);
|
|
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);
|
|
i2c_envoi_32bits(planche_centre_x, 1);
|
|
i2c_envoi_32bits(planche_centre_y, 5);
|
|
i2c_envoi_32bits((int)(planche_angle_rad * 1000), 9);
|
|
context.mem[0] = 2;
|
|
}
|
|
|
|
|
|
}
|
|
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);
|
|
printf(">gauche_planche_angle:%f\n",gauche_planche_angle / M_PI * 180);
|
|
printf(">droit_planche_angle:%f\n",droit_planche_angle / M_PI * 180);
|
|
printf(">echec:%d\n", echec);
|
|
|
|
|
|
} |