File content as of revision 11:da3c8e9bf1f9:

/*-----------------------------------------------------------


 */

#include <stdio.h>

#include "mbed.h"
#include "XNucleo53L1A1.h"
#include "VL53L1X_I2C.h"
#include "rtos.h"
#include "XNucleoIKS01A3.h"

#define VL53L1_I2C_SDA   D5  //D14
#define VL53L1_I2C_SCL   D7 //D15

#define DISPLAY_CENTRE false
#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;

    int status = 0;
    uint8_t ready_centre = 0;
    uint8_t ready_left = 0;
    uint8_t ready_right = 0;
    uint16_t distance_centre = 0;
    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);

    printf("I2C device created!\r\n");

    /* creates the 53L1A1 expansion board singleton obj */
    //board = XNucleo53L1A1::instance(dev_I2C, A2, D9, D2);    /* original code */
    board = XNucleo53L1A1::instance(dev_I2C, A2, PB_13, PB_14);
    printf("board created!\r\n");

    /* init the 53L1A1 expansion board with default values */
    status = board->init_board();
    if (status) {
        printf("Failed to init board!\r\n");
        return status;
    }

    printf("board initiated! - %d\r\n", status);

    /* 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;
        }
    }

    /* 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;
        }
    }

    /* 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
     */
     
         // 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_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);
        }  
     }
     
     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) {
            printf("Distance read by centre sensor is : %d mm\r\n", distance_centre);
        }
        #endif
        #if DISPLAY_LEFT
        if (board->sensor_left != NULL) {
            printf("Distance read by left satellite sensor is : %d mm\r\n", distance_left);
        }
        #endif
        #if DISPLAY_RIGHT
        if (board->sensor_right != NULL) {
            printf("Distance read by right satellite sensor is : %d mm\r\n", distance_right);
        }
        #endif

    }

    return status;
}