Diff: main.cpp

Revision:

11:da3c8e9bf1f9

Parent:

6:054c18d427bf

--- 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;