8x8 data @15Hz + sketch for processing

This commit is contained in:
Samuel 2023-10-22 18:25:57 +02:00
parent 19d50b56fa
commit 06e6ec14b3
6 changed files with 220 additions and 99 deletions

37
.vscode/c_cpp_properties.json vendored Normal file
View File

@ -0,0 +1,37 @@
{
"env": {
"myDefaultIncludePath": [
"${workspaceFolder}",
"${workspaceFolder}/VL53L8CX_ULD_API/inc",
"${workspaceFolder}/build",
"${env:PICO_SDK_PATH}/src/**/include",
"${env:PICO_SDK_PATH}/src/common/pico_base/include",
"${env:PICO_SDK_PATH}/build/generated/pico_base",
"${env:PICO_SDK_PATH}/src/common/pico_base/include/pico",
"${env:PICO_SDK_PATH}/src/common/pico_stdlib/include"
],
"myCompilerPath": "/usr/bin/arm-none-eabi-gcc"
},
"configurations": [
{
"name": "Linux",
"intelliSenseMode": "linux-gcc-arm",
"includePath": [
"${myDefaultIncludePath}",
"${workspaceFolder}/build/"
],
"compilerPath": "/usr/bin/arm-none-eabi-gcc",
"cStandard": "c11",
"cppStandard": "c++17",
"browse": {
"path": [
"${workspaceFolder}"
],
"limitSymbolsToIncludedHeaders": true,
"databaseFilename": ""
},
"configurationProvider": "ms-vscode.cmake-tools"
}
],
"version": 4
}

25
.vscode/tasks.json vendored Normal file
View File

@ -0,0 +1,25 @@
{
"tasks": [
{
"type": "shell",
"command": "cd build; cmake ../; make",
"label": "CMake in build/",
"problemMatcher": [],
"group": {
"kind": "build",
"isDefault": false
}
},
{
"type": "shell",
"command": "cd build; cmake ../; make Flash",
"label": "CMake & Make & Flash",
"problemMatcher": [],
"group": {
"kind": "build",
"isDefault": true
}
}
],
"version": "2.0.0"
}

View File

@ -0,0 +1,110 @@
/**
* Simple Read
*
* Read data from the serial port and change the color of a rectangle
* when a switch connected to a Wiring or Arduino board is pressed and released.
* This example works with the Wiring / Arduino program that follows below.
*/
import processing.serial.*;
Serial myPort; // Create object from Serial class
int val; // Data received from the serial port
int square_size = 40;
int square_gap = 4;
int square_space = (square_size + square_gap);
int max_distance = 650;
int max_color = 650;
void setup()
{
size(360, 360);
colorMode(HSB, 1000);
// I know that the first port in the serial list on my mac
// is always my FTDI adaptor, so I open Serial.list()[0].
// On Windows machines, this generally opens COM1.
// Open whatever port is the one you're using.
print(Serial.list());
String portName = Serial.list()[0];
myPort = new Serial(this, portName, 115200);
}
boolean record_started=false;
String data="";
void draw()
{
if ( myPort.available() > 0) { // If data is available,
String inBuffer = myPort.readString();
String[] lines = inBuffer.split("\n");
for(int index = 0; index<lines.length; index++){
if ( lines[index].startsWith(">distance:")){
data = lines[index];
record_started=true;
continue;
}
if(record_started){
data += lines[index];
if(count(data) >= 64){
record_started=false;
display_my_data(data);
}
}
}
}
}
int count(String line){
return line.length() - line.replace(",", "").length();
}
void display_my_data(String data){
String distances[] = data.split(":")[1].split(",");
print(distances[0]);
for(int row=0; row < 8; row++){
for(int col=0; col < 8; col++){
int distance = int(distances[row+8*col]);
if( distance >max_distance)distance = max_distance;
fill(color(distance * max_color/ max_distance, 1000, 1000));
square(row * square_space + square_gap, col * square_space + square_gap, square_size);
}
}
}
/*
// Wiring / Arduino Code
// Code for sensing a switch status and writing the value to the serial port.
int switchPin = 4; // Switch connected to pin 4
void setup() {
pinMode(switchPin, INPUT); // Set pin 0 as an input
Serial.begin(9600); // Start serial communication at 9600 bps
}
void loop() {
if (digitalRead(switchPin) == HIGH) { // If switch is ON,
Serial.write(1); // send 1 to Processing
} else { // If the switch is not ON,
Serial.write(0); // send 0 to Processing
}
delay(100); // Wait 100 milliseconds
}
*/

View File

@ -1,47 +1,6 @@
Détection de l'adversaire - 2023
================================
VL53L8CX with the Raspberry Pi Pico (RP2040)
============================================
Le code présenté ici sert à la détection de l'adversaire pour les robots de l'équipe [Poivron Robotique](http://poivron-robotique.fr) qui participe à la coupe de France de Robotique. Ce code tourne sur un Raspberry Pi Pico et une carte personnalisé sur laquelle sont banchés 12 capteurs VL53L1X.
This code use a Raspberry Pi Pico (RP2040) to read the data of a VL53L8CX.
Principe de fonctionnement
--------------------------
Les capteurs VL53L1X sont des capteurs I2C dont l'adresse est configurable. Cependant, le capteur revient à son adresse par défaut dès qu'il est éteint ou désactivé par la broche `XSHUT`. Le Raspberry Pi Pico est relié à un 74HC4067, qui lui permet de désactiver un capteur à la fois.
Le code utilisé ici repose sur [VL53L1X_Raspberry_Pi_Pico](https://git.poivron-robotique.fr/Keuronde/Raspberry_Pi_Pico_VL53L1X) et l'exemple pour piloter les LED RGB ws2812 fourni avec le SDK du Raspberry Pi Pico.
Initialisation
--------------
La première étape de l'initialisation consiste à changer tous les capteurs d'adresses en une seule commande I2C.
La seconde étape consiste, pour chaque capteur, à le désactiver puis à le réactiver à l'aide du 74HC4067 et de la boche `XSHUT` du composant. Ce faisant, il reprend son adresse par défaut et il est le seul à cette adresse. Ainsi il est possible, via le bus I2C de lui attribuer une adresse unique.
Communication I2C
-----------------
Avec le reste du robot, la carte communique en I2C sur le bus i2c1, se comportant comme une EPROM en utilisant le code de Valentin Milea <valentin.milea@gmail.com>, [Pico_i2c_slave](https://github.com/vmilea/pico_i2c_slave).
Description de la mémoire d'échange:
------------------------------------
Les adresses 0 à 11 (0x0B) contiennent les distances des capteurs en centimètre. Cette distance est saturée à 200 cm.
Les adresses 16 (0x10) à 18 (0x12) contiennent les consignes d'éclairage des LEDs:
* 0x10 : Couleur sur 8 bits.
* 0x11 : octet de poids fort pour le masque sélectionnant les LEDs à commander.
* 0x12 : octet de poids faible pour le masque sélectionnant les LEDs à commander.
Couleur sur 8 bits structurée ainsi : 3-3-2 (Rouge - vert - bleu).
Le masque sur 16 bits, 1 : la led doit être piloter de la couleur indiquée, 0 la led affiche la distance.
Valeur spéciale pour le masque : 0x00. Dans ce cas, toutes les LEDs reprennent leur fonction par défaut (affichage de la distance).
Démo - Ne fonctionne plus
----
La démo ne fonctionne plus, la carte initialise automatiquement les capteurs et lit leurs valeurs en continu. Le code esclave I2C envoi ces valeurs qui sont stockée de l'adresse 0x0 à 0x0B de la "mémoire" I2C.
Le code I2C pour la partie esclave est repris de [pico_i2c_slave](https://github.com/vmilea/pico_i2c_slave).
Une fois branchée à un ordinateur, le Raspberry Pi Pico propose un menu par la liaison USB/série.
- La commande "i" initialise les capteurs et change d'état les LEDs en fonction du succès de l'initialisation.
- La commande "j" démarre une lecture continue et renvoie les données des capteurs dans un format compatible avec [Teleplot](https://github.com/nesnes/teleplot/).
Current status : ability to read @15Hz the 8x8 values and send them to a computer through the USB connection. Based mainly on the "example 1" of the "Ultra Lite Driver" provided by ST Microelectronics.

View File

@ -12,7 +12,6 @@
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include "vl53l8cx_api.h"
#include "vl53l8cx_buffers.h"
@ -224,15 +223,10 @@ uint8_t vl53l8cx_is_alive(
uint8_t status = VL53L8CX_STATUS_OK;
uint8_t device_id, revision_id;
printf("Alive 1\n");
status |= WrByte(&(p_dev->platform), 0x7fff, 0x00);
printf("Alive 2\n");
status |= RdByte(&(p_dev->platform), 0, &device_id);
printf("Alive 3\n");
status |= RdByte(&(p_dev->platform), 1, &revision_id);
printf("Alive 4\n");
status |= WrByte(&(p_dev->platform), 0x7fff, 0x02);
printf("Alive 5\n");
if (!status) return status;
@ -261,7 +255,6 @@ uint8_t vl53l8cx_init(
p_dev->is_auto_stop_enabled = (uint8_t)0x0;
/* SW reboot sequence */
printf("Init 0\n");
status |= WrByte(&(p_dev->platform), 0x7fff, 0x00);
status |= WrByte(&(p_dev->platform), 0x0009, 0x04);
status |= WrByte(&(p_dev->platform), 0x000F, 0x40);
@ -269,8 +262,6 @@ uint8_t vl53l8cx_init(
status |= RdByte(&(p_dev->platform), 0x7FFF, &tmp);
status |= WrByte(&(p_dev->platform), 0x000C, 0x01);
printf("Init 1, %d\n", status);
status |= WrByte(&(p_dev->platform), 0x0101, 0x00);
status |= WrByte(&(p_dev->platform), 0x0102, 0x00);
status |= WrByte(&(p_dev->platform), 0x010A, 0x01);
@ -282,22 +273,17 @@ uint8_t vl53l8cx_init(
status |= WrByte(&(p_dev->platform), 0x000F, 0x43);
status |= WaitMs(&(p_dev->platform), 1);
printf("Init 2, %d\n", status);
status |= WrByte(&(p_dev->platform), 0x000F, 0x40);
status |= WrByte(&(p_dev->platform), 0x000A, 0x01);
status |= WaitMs(&(p_dev->platform), 100);
printf("Init 3, %d\n", status);
/* Wait for sensor booted (several ms required to get sensor ready ) */
status |= WrByte(&(p_dev->platform), 0x7fff, 0x00);
status |= _vl53l8cx_poll_for_answer(p_dev, 1, 0, 0x06, 0xff, 1);
if(status != (uint8_t)0){
goto exit;
}
printf("Init 4, %d\n", status);
status |= WrByte(&(p_dev->platform), 0x000E, 0x01);
status |= WrByte(&(p_dev->platform), 0x7fff, 0x02);
@ -308,14 +294,10 @@ uint8_t vl53l8cx_init(
status |= WrByte(&(p_dev->platform), 0x7fff, 0x00);
printf("Init 5, %d\n", status);
/* Enable host access to GO1 */
status |= RdByte(&(p_dev->platform), 0x7fff, &tmp);
status |= WrByte(&(p_dev->platform), 0x0C, 0x01);
printf("Init 6, %d\n", status);
/* Power ON status */
status |= WrByte(&(p_dev->platform), 0x7fff, 0x00);
status |= WrByte(&(p_dev->platform), 0x101, 0x00);
@ -333,8 +315,6 @@ uint8_t vl53l8cx_init(
status |= WrByte(&(p_dev->platform), 0x219, 0x00);
status |= WrByte(&(p_dev->platform), 0x21B, 0x00);
printf("Init 7, %d\n", status);
/* Wake up MCU */
status |= WrByte(&(p_dev->platform), 0x7fff, 0x00);
status |= RdByte(&(p_dev->platform), 0x7fff, &tmp);
@ -342,28 +322,18 @@ uint8_t vl53l8cx_init(
status |= WrByte(&(p_dev->platform), 0x20, 0x07);
status |= WrByte(&(p_dev->platform), 0x20, 0x06);
printf("Init 8, %d\n", status);
/* Download FW into VL53L8CX */
status |= WrByte(&(p_dev->platform), 0x7fff, 0x09);
printf("Init 9, %d\n", status);
status |= WrMulti(&(p_dev->platform),0,
(uint8_t*)&VL53L8CX_FIRMWARE[0],0x8000);
printf("Init 10, %d\n", status);
status |= WrByte(&(p_dev->platform), 0x7fff, 0x0a);
printf("Init 11, %d\n", status);
status |= WrMulti(&(p_dev->platform),0,
(uint8_t*)&VL53L8CX_FIRMWARE[0x8000],0x8000);
printf("Init 12, %d\n", status);
status |= WrByte(&(p_dev->platform), 0x7fff, 0x0b);
printf("Init 13, %d\n", status);
status |= WrMulti(&(p_dev->platform),0,
(uint8_t*)&VL53L8CX_FIRMWARE[0x10000],0x5000);
printf("Init 14, %d\n", status);
status |= WrByte(&(p_dev->platform), 0x7fff, 0x01);
printf("Init 15, %d\n", status);
/* Check if FW correctly downloaded */
status |= WrByte(&(p_dev->platform), 0x7fff, 0x01);
status |= WrByte(&(p_dev->platform), 0x06, 0x03);
@ -373,8 +343,6 @@ uint8_t vl53l8cx_init(
status |= RdByte(&(p_dev->platform), 0x7fff, &tmp);
status |= WrByte(&(p_dev->platform), 0x0C, 0x01);
printf("Init 16, %d\n", status);
/* Reset MCU and wait boot */
status |= WrByte(&(p_dev->platform), 0x7FFF, 0x00);
status |= WrByte(&(p_dev->platform), 0x114, 0x00);
@ -386,8 +354,6 @@ uint8_t vl53l8cx_init(
status |= WrByte(&(p_dev->platform), 0x0C, 0x00);
status |= WrByte(&(p_dev->platform), 0x0B, 0x01);
printf("Init 1, %d\n", status);
status |= _vl53l8cx_poll_for_mcu_boot(p_dev);
if(status != (uint8_t)0){
goto exit;
@ -432,7 +398,6 @@ uint8_t vl53l8cx_init(
status |= vl53l8cx_dci_write_data(p_dev, (uint8_t*)&pipe_ctrl,
VL53L8CX_DCI_PIPE_CONTROL, (uint16_t)sizeof(pipe_ctrl));
printf("Init 1, %d\n", status);
#if VL53L8CX_NB_TARGET_PER_ZONE != 1
tmp = VL53L8CX_NB_TARGET_PER_ZONE;
status |= vl53l8cx_dci_replace_data(p_dev, p_dev->temp_buffer,
@ -444,8 +409,6 @@ uint8_t vl53l8cx_init(
VL53L8CX_DCI_SINGLE_RANGE,
(uint16_t)sizeof(single_range));
printf("Init 1, %d\n", status);
exit:
return status;
}

59
main.c
View File

@ -87,7 +87,7 @@ void main(void)
stdio_init_all();
gpio_set_function(0, GPIO_FUNC_I2C);
gpio_set_function(1, GPIO_FUNC_I2C);
i2c_init(i2c0, 100 * 1000);
i2c_init(i2c0, 400 * 1000);
printf("3\n");
WaitMs(0, 1000);
printf("2\n");
@ -153,6 +153,25 @@ void main(void)
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);
/*********************************/
@ -162,43 +181,51 @@ void main(void)
status = vl53l8cx_start_ranging(&Dev);
loop = 0;
uint32_t time0, time1, time2, time3;
while(1)
{
/* Use polling function to know when a new measurement is ready.
* Another way can be to wait for HW interrupt raised on PIN A1
* (INT) when a new measurement is ready */
time0 = time_us_32();
status = vl53l8cx_check_data_ready(&Dev, &isReady);
time1 = time_us_32();
if(isReady)
{
vl53l8cx_get_ranging_data(&Dev, &Results);
time2 = time_us_32();
/* 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 */
printf("Print data no : %3u\n", Dev.streamcount);
/*for(i = 0; i < 16; i++)
{
printf("Zone : %3d, Status : %3u, Distance : %4d mm\n",
i,
Results.target_status[VL53L8CX_NB_TARGET_PER_ZONE*i],
Results.distance_mm[VL53L8CX_NB_TARGET_PER_ZONE*i]);
}*/
printf("%4d,%4d,%4d,%4d\n%4d,%4d,%4d,%4d\n%4d,%4d,%4d,%4d\n%4d,%4d,%4d,%4d\n",
Results.distance_mm[0],Results.distance_mm[1],Results.distance_mm[2],Results.distance_mm[3],
Results.distance_mm[4],Results.distance_mm[5],Results.distance_mm[6],Results.distance_mm[7],
Results.distance_mm[8],Results.distance_mm[9],Results.distance_mm[10],Results.distance_mm[11],
Results.distance_mm[12],Results.distance_mm[13],Results.distance_mm[14],Results.distance_mm[15]
);
printf(">distance:");
for(int row=0; row < 8; row++){
for(int col=0; col < 8; col++){
if(Results.target_status[col+ 8*row] == 255 ||// No Target detected
Results.target_status[col+ 8*row] == 10 ||// Range valid, but no target detected at previous range
Results.target_status[col+ 8*row] == 4 ||// Target consistency failed
Results.target_status[col+ 8*row] == 3 ){// Sigma error too high
Results.distance_mm[col+ 8*row] = 3000;
}
printf("%04d,", Results.distance_mm[col+ 8*row]);
if((row == 0 ||row == 1 ) && Results.distance_mm[col+ 8*row] < 650){
// printf("\nStatus: %d\n",Results.target_status[col+ 8*row]);
}
}
}
printf("\n");
time3 = time_us_32();
loop++;
printf("temps : %d, %d, %d\n", time1-time0, time2-time0, time3-time0);
}
/* Wait a few ms to avoid too high polling (function in platform
* file, not in API) */
WaitMs(&(Dev.platform), 5);
//WaitMs(&(Dev.platform), 5);
}
status = vl53l8cx_stop_ranging(&Dev);