diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..2996c7e --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -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 +} \ No newline at end of file diff --git a/.vscode/tasks.json b/.vscode/tasks.json new file mode 100644 index 0000000..c98bf9b --- /dev/null +++ b/.vscode/tasks.json @@ -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" +} \ No newline at end of file diff --git a/Processing/VL53L8CX_8x8/VL53L8CX_8x8.pde b/Processing/VL53L8CX_8x8/VL53L8CX_8x8.pde new file mode 100644 index 0000000..7679885 --- /dev/null +++ b/Processing/VL53L8CX_8x8/VL53L8CX_8x8.pde @@ -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; indexdistance:")){ + 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 +} + +*/ diff --git a/Readme.md b/Readme.md index 943b6f1..2b9875e 100644 --- a/Readme.md +++ b/Readme.md @@ -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 , [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. \ No newline at end of file diff --git a/VL53L8CX_ULD_API/src/vl53l8cx_api.c b/VL53L8CX_ULD_API/src/vl53l8cx_api.c index 91892f8..6191b79 100644 --- a/VL53L8CX_ULD_API/src/vl53l8cx_api.c +++ b/VL53L8CX_ULD_API/src/vl53l8cx_api.c @@ -12,7 +12,6 @@ #include #include -#include #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; } diff --git a/main.c b/main.c index 10205aa..0b591ea 100644 --- a/main.c +++ b/main.c @@ -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);