![](/media/cache/group/HuMVe-1_logo.png.50x50_q85.png)
Second demo with X-NUCLEO-53L1A1.
Dependencies: X_NUCLEO_53L1A1 X_NUCLEO_IKS01A3
Diff: main.cpp
- Revision:
- 11:da3c8e9bf1f9
- Parent:
- 6:054c18d427bf
diff -r 2b50d0170895 -r da3c8e9bf1f9 main.cpp --- a/main.cpp Mon Feb 14 16:36:46 2022 +0000 +++ b/main.cpp Thu Mar 24 17:14:56 2022 +0000 @@ -1,32 +1,6 @@ /*----------------------------------------------------------- - Demo usata nel Corso STM32 NUCLEO Mbed OS - Novembre 2021 ------------------------------------------------------------ -*/ -/* - * This VL53L1X Expansion board test application performs range measurements - * using the onboard embedded centre sensor, and 2 Satellite boards. - * - * Measured ranges are ouput on the Serial Port, running at 9600 baud. - * - * The Black Reset button is used to restart the program. - * - * *** NOTE : By default hardlinks U10, U11, U15 & U18, on the underside of - * the X-NUCELO-53L0A1 expansion board are not made/OFF. - * These links must be made to allow interrupts from the Satellite boards - * to be received. - * U11 and U18 must be made/ON to allow interrupts to be received from the - * INT_L & INT_R positions; or - * U10 and U15 must be made/ON to allow interrupts to be received from the - * Alternate INT_L & INT_R positions. - * The X_NUCLEO_53L1A1 firmware library defaults to use the INT_L/INT_R - * positions. - * INT_L is available on expansion board Arduino Connector CN5, pin 1 as D9. - * Alternate INT_L is on CN5 Connector pin 2 as D8. - * INT_R is available on expansion board Arduino Connector CN9, pin 3 as D4. - * Alternate INT_R is on CN9 Connector pin 5 as D2. - * The pinouts are shown here : https://developer.mbed.org/components/X-NUCLEO-53L1A1/ */ #include <stdio.h> @@ -34,22 +8,39 @@ #include "mbed.h" #include "XNucleo53L1A1.h" #include "VL53L1X_I2C.h" +#include "rtos.h" +#include "XNucleoIKS01A3.h" -#define VL53L1_I2C_SDA D14 -#define VL53L1_I2C_SCL D15 +#define VL53L1_I2C_SDA D5 //D14 +#define VL53L1_I2C_SCL D7 //D15 #define DISPLAY_CENTRE false -#define DISPLAY_LEFT true +#define DISPLAY_LEFT false #define DISPLAY_RIGHT false +// LED blink +#define LED_FREQUENCY 0.5 //frequenza di blink del led +#define LED_OFF led =0 +#define LED_ON led =1 +#define LED_DURATA 100 // durata del blink + +// Time Of Flight +#define ToF_FREQUENCY 0.2 // frequenza di controllo della distanza +#define DISPLAY_LEFT false +#define DISPLAY_RIGHT false +#define ToF_SOGLIA_CRITICA 20 //mm sotto la quale scatta +#define ToF_ITERATIONS 50 //iterazioni per la calibrazione + +// ToF static XNucleo53L1A1 *board=NULL; - +// mems +/* Instantiate the expansion board */ +static XNucleoIKS01A3 *mems_expansion_board = XNucleoIKS01A3::instance(D14, D15, D4, D5, A3, D6, A4); +/* Retrieve the composing elements of the expansion board */ +static LSM6DSOSensor *acc_gyro = mems_expansion_board->acc_gyro; +static LIS2DW12Sensor *accelerometer = mems_expansion_board->accelerometer; -/*=================================== Main ================================== -=============================================================================*/ -int main() -{ int status = 0; uint8_t ready_centre = 0; uint8_t ready_left = 0; @@ -58,6 +49,66 @@ uint16_t distance_left = 0; uint16_t distance_right = 0; +DigitalOut led(LED1); // define the LED pin +Ticker myTick; // create a ticker object +Ticker Tick_ToF; +bool checktof =0; +bool stop_misura=0; + +void onTick(void) { // function to call every tick + led = !led; // toggle the LED +} + +void check_ToF(void) +{ + checktof = 1; +} + + + +/* Helper function for printing floats & doubles */ +static char *print_double(char *str, double v, int decimalDigits = 2) +{ + int i = 1; + int intPart, fractPart; + int len; + char *ptr; + + /* prepare decimal digits multiplicator */ + for (; decimalDigits != 0; i *= 10, decimalDigits--); + + /* calculate integer & fractinal parts */ + intPart = (int)v; + fractPart = (int)((v - (double)(int)v) * i); + + /* fill in integer part */ + sprintf(str, "%i.", intPart); + + /* prepare fill in of fractional part */ + len = strlen(str); + ptr = &str[len]; + + /* fill in leading fractional zeros */ + for (i /= 10; i > 1; i /= 10, ptr++) { + if (fractPart >= i) { + break; + } + *ptr = '0'; + } + + /* fill in (rest of) fractional part */ + sprintf(ptr, "%i", fractPart); + + return str; +} + + +/*=================================== Main ================================== +=============================================================================*/ +int main() +{ + + printf("Hello world!\r\n"); VL53L1X_DevI2C *dev_I2C = new VL53L1X_DevI2C(VL53L1_I2C_SDA, VL53L1_I2C_SCL); @@ -81,6 +132,7 @@ /* Start ranging on the centre sensor */ if (board->sensor_centre != NULL) { status = board->sensor_centre->vl53l1x_start_ranging(); + board->sensor_centre->disable_interrupt_measure_detection_irq(); if (status != 0) { printf("Centre sensor failed to start ranging!\r\n"); return status; @@ -90,6 +142,7 @@ /* Start ranging on the left sensor */ if (board->sensor_left != NULL) { status = board->sensor_left->vl53l1x_start_ranging(); + board->sensor_left->disable_interrupt_measure_detection_irq(); if (status != 0) { printf("Left sensor failed to start ranging!\r\n"); return status; @@ -99,30 +152,46 @@ /* Start ranging on the right sensor */ if (board->sensor_right != NULL) { status = board->sensor_right->vl53l1x_start_ranging(); + board->sensor_right->disable_interrupt_measure_detection_irq(); if (status != 0) { printf("Right sensor failed to start ranging!\r\n"); return status; } } + +//// mems + // initialize mems + uint8_t id; + float value1, value2; + char buffer1[32], buffer2[32]; + int32_t axes[3]; + + /* Enable all sensors */ + accelerometer->enable_x(); + acc_gyro->enable_x(); + acc_gyro->enable_g(); + accelerometer->read_id(&id); + printf("LIS2DW12 accelerometer = 0x%X\r\n", id); + acc_gyro->read_id(&id); + printf("LSM6DSO accelerometer & gyroscope = 0x%X\r\n", id); + + board = XNucleo53L1A1::instance(dev_I2C); + printf("I2C device created!\r\n"); + + /* Ranging loop * Checks each sensor for data ready */ - while (1) - { - if (board->sensor_centre != NULL) { - board->sensor_centre->vl53l1x_check_for_data_ready(&ready_centre); - } + + // calibration for ToF + for (int j=0; j<=ToF_ITERATIONS; j++) { if (board->sensor_left != NULL) { board->sensor_left->vl53l1x_check_for_data_ready(&ready_left); } if (board->sensor_right != NULL) { board->sensor_right->vl53l1x_check_for_data_ready(&ready_right); } - if (ready_centre) { - board->sensor_centre->vl53l1x_get_range_status(&ready_centre); - board->sensor_centre->vl53l1x_get_distance(&distance_centre); - } if (ready_left) { board->sensor_left->vl53l1x_get_range_status(&ready_left); board->sensor_left->vl53l1x_get_distance(&distance_left); @@ -130,6 +199,58 @@ if (ready_right) { board->sensor_right->vl53l1x_get_range_status(&ready_right); board->sensor_right->vl53l1x_get_distance(&distance_right); + } + } + + printf("calibration done\r\n"); + + // start check ToF + Tick_ToF.attach(&check_ToF,ToF_FREQUENCY); + + while (1) + { + led = 0; + if (checktof ==1) { + if (board->sensor_left != NULL) { + board->sensor_left->vl53l1x_check_for_data_ready(&ready_left); + } + if (board->sensor_right != NULL) { + board->sensor_right->vl53l1x_check_for_data_ready(&ready_right); + } + if (ready_left) { + board->sensor_left->vl53l1x_get_range_status(&ready_left); + board->sensor_left->vl53l1x_get_distance(&distance_left); + } + if (ready_right) { + board->sensor_right->vl53l1x_get_range_status(&ready_right); + board->sensor_right->vl53l1x_get_distance(&distance_right); + } + checktof =0; + if (distance_left <= ToF_SOGLIA_CRITICA || distance_right <= ToF_SOGLIA_CRITICA) { + led = 1; + //printf("\r\n DISTANZA PIU' PICCOLA DELLA SOGLIA !!!"); + //ThisThread::sleep_for(1000); + stop_misura =1; + } else { + stop_misura =0; + } + } + if (stop_misura) { + printf("\r\n DISTANZA PIU' PICCOLA DELLA SOGLIA !!!"); + } else { + + accelerometer->get_x_axes(axes); + printf("LIS2DW12 [acc/mg]: %6d, %6d, %6d\r\n", axes[0], axes[1], axes[2]); + + acc_gyro->get_x_axes(axes); + printf("LSM6DSO [acc/mg]: %6d, %6d, %6d\r\n", axes[0], axes[1], axes[2]); + + acc_gyro->get_g_axes(axes); + printf("LSM6DSO [gyro/mdps]: %6d, %6d, %6d\r\n", axes[0], axes[1], axes[2]); + //ThisThread::sleep_for(1000); + + + } #if DISPLAY_CENTRE if (board->sensor_centre != NULL) { @@ -146,6 +267,7 @@ printf("Distance read by right satellite sensor is : %d mm\r\n", distance_right); } #endif + } return status;