main.cpp

00001 /*-----------------------------------------------------------
00002 
00003 
00004  */
00005 
00006 #include <stdio.h>
00007 
00008 #include "mbed.h"
00009 #include "XNucleo53L1A1.h"
00010 #include "VL53L1X_I2C.h"
00011 #include "rtos.h"
00012 #include "XNucleoIKS01A3.h"
00013 
00014 #define VL53L1_I2C_SDA   D5  //D14
00015 #define VL53L1_I2C_SCL   D7 //D15
00016 
00017 #define DISPLAY_CENTRE false
00018 #define DISPLAY_LEFT   false
00019 #define DISPLAY_RIGHT  false
00020 
00021 // LED blink
00022 #define LED_FREQUENCY 0.5 //frequenza di blink del led
00023 #define LED_OFF led =0
00024 #define LED_ON led =1
00025 #define LED_DURATA 100 // durata del blink
00026 
00027 // Time Of Flight
00028 #define ToF_FREQUENCY 0.2  // frequenza di controllo della distanza
00029 #define DISPLAY_LEFT   false
00030 #define DISPLAY_RIGHT  false
00031 #define ToF_SOGLIA_CRITICA 20 //mm sotto la quale scatta 
00032 #define ToF_ITERATIONS 50 //iterazioni per la calibrazione
00033 
00034 // ToF
00035 static XNucleo53L1A1 *board=NULL;
00036 
00037 // mems
00038 /* Instantiate the expansion board */
00039 static XNucleoIKS01A3 *mems_expansion_board = XNucleoIKS01A3::instance(D14, D15, D4, D5, A3, D6, A4);
00040 /* Retrieve the composing elements of the expansion board */
00041 static LSM6DSOSensor *acc_gyro = mems_expansion_board->acc_gyro;
00042 static LIS2DW12Sensor *accelerometer = mems_expansion_board->accelerometer;
00043 
00044     int status = 0;
00045     uint8_t ready_centre = 0;
00046     uint8_t ready_left = 0;
00047     uint8_t ready_right = 0;
00048     uint16_t distance_centre = 0;
00049     uint16_t distance_left = 0;
00050     uint16_t distance_right = 0;
00051 
00052 DigitalOut led(LED1); // define the LED pin
00053 Ticker myTick;            // create a ticker object
00054 Ticker Tick_ToF;
00055 bool checktof =0;
00056 bool stop_misura=0;
00057 
00058 void onTick(void) {     // function to call every tick
00059   led = !led;                 //  toggle the LED
00060 }
00061 
00062 void check_ToF(void)
00063 {
00064  checktof = 1;
00065 }
00066 
00067 
00068 
00069 /* Helper function for printing floats & doubles */
00070 static char *print_double(char *str, double v, int decimalDigits = 2)
00071 {
00072     int i = 1;
00073     int intPart, fractPart;
00074     int len;
00075     char *ptr;
00076 
00077     /* prepare decimal digits multiplicator */
00078     for (; decimalDigits != 0; i *= 10, decimalDigits--);
00079 
00080     /* calculate integer & fractinal parts */
00081     intPart = (int)v;
00082     fractPart = (int)((v - (double)(int)v) * i);
00083 
00084     /* fill in integer part */
00085     sprintf(str, "%i.", intPart);
00086 
00087     /* prepare fill in of fractional part */
00088     len = strlen(str);
00089     ptr = &str[len];
00090 
00091     /* fill in leading fractional zeros */
00092     for (i /= 10; i > 1; i /= 10, ptr++) {
00093         if (fractPart >= i) {
00094             break;
00095         }
00096         *ptr = '0';
00097     }
00098 
00099     /* fill in (rest of) fractional part */
00100     sprintf(ptr, "%i", fractPart);
00101 
00102     return str;
00103 }
00104 
00105 
00106 /*=================================== Main ==================================
00107 =============================================================================*/
00108 int main()
00109 {
00110 
00111 
00112     printf("Hello world!\r\n");
00113 
00114     VL53L1X_DevI2C *dev_I2C = new VL53L1X_DevI2C(VL53L1_I2C_SDA, VL53L1_I2C_SCL);
00115 
00116     printf("I2C device created!\r\n");
00117 
00118     /* creates the 53L1A1 expansion board singleton obj */
00119     //board = XNucleo53L1A1::instance(dev_I2C, A2, D9, D2);    /* original code */
00120     board = XNucleo53L1A1::instance(dev_I2C, A2, PB_13, PB_14);
00121     printf("board created!\r\n");
00122 
00123     /* init the 53L1A1 expansion board with default values */
00124     status = board->init_board();
00125     if (status) {
00126         printf("Failed to init board!\r\n");
00127         return status;
00128     }
00129 
00130     printf("board initiated! - %d\r\n", status);
00131 
00132     /* Start ranging on the centre sensor */
00133     if (board->sensor_centre != NULL) {
00134         status = board->sensor_centre->vl53l1x_start_ranging();
00135         board->sensor_centre->disable_interrupt_measure_detection_irq();
00136         if (status != 0) {
00137             printf("Centre sensor failed to start ranging!\r\n");
00138             return status;
00139         }
00140     }
00141 
00142     /* Start ranging on the left sensor */
00143     if (board->sensor_left != NULL) {
00144         status = board->sensor_left->vl53l1x_start_ranging();
00145         board->sensor_left->disable_interrupt_measure_detection_irq();
00146         if (status != 0) {
00147             printf("Left sensor failed to start ranging!\r\n");
00148             return status;
00149         }
00150     }
00151 
00152     /* Start ranging on the right sensor */
00153     if (board->sensor_right != NULL) {
00154         status = board->sensor_right->vl53l1x_start_ranging();
00155         board->sensor_right->disable_interrupt_measure_detection_irq();
00156         if (status != 0) {
00157             printf("Right sensor failed to start ranging!\r\n");
00158             return status;
00159         }
00160     }
00161 
00162 
00163 //// mems
00164   // initialize mems
00165     uint8_t id;
00166     float value1, value2;
00167     char buffer1[32], buffer2[32];
00168     int32_t axes[3];
00169 
00170     /* Enable all sensors */
00171     accelerometer->enable_x();
00172     acc_gyro->enable_x();
00173     acc_gyro->enable_g();
00174     accelerometer->read_id(&id);
00175     printf("LIS2DW12 accelerometer            = 0x%X\r\n", id);
00176     acc_gyro->read_id(&id);
00177     printf("LSM6DSO accelerometer & gyroscope = 0x%X\r\n", id);
00178     
00179     board = XNucleo53L1A1::instance(dev_I2C);
00180     printf("I2C device created!\r\n");
00181 
00182 
00183     /* Ranging loop
00184      * Checks each sensor for data ready
00185      */
00186      
00187          // calibration for ToF
00188      for (int j=0; j<=ToF_ITERATIONS; j++) {
00189         if (board->sensor_left != NULL) {
00190             board->sensor_left->vl53l1x_check_for_data_ready(&ready_left);
00191         }
00192         if (board->sensor_right != NULL) {
00193             board->sensor_right->vl53l1x_check_for_data_ready(&ready_right);
00194         }
00195         if (ready_left) {
00196             board->sensor_left->vl53l1x_get_range_status(&ready_left);
00197             board->sensor_left->vl53l1x_get_distance(&distance_left);
00198         }
00199         if (ready_right) {
00200             board->sensor_right->vl53l1x_get_range_status(&ready_right);
00201             board->sensor_right->vl53l1x_get_distance(&distance_right);
00202         }  
00203      }
00204      
00205      printf("calibration done\r\n");
00206      
00207     // start check ToF
00208     Tick_ToF.attach(&check_ToF,ToF_FREQUENCY);
00209       
00210     while (1)
00211     {
00212         led = 0;
00213         if (checktof ==1) {
00214           if (board->sensor_left != NULL) {
00215             board->sensor_left->vl53l1x_check_for_data_ready(&ready_left);
00216           }
00217           if (board->sensor_right != NULL) {
00218             board->sensor_right->vl53l1x_check_for_data_ready(&ready_right);
00219           }
00220           if (ready_left) {
00221             board->sensor_left->vl53l1x_get_range_status(&ready_left);
00222             board->sensor_left->vl53l1x_get_distance(&distance_left);
00223           }
00224           if (ready_right) {
00225             board->sensor_right->vl53l1x_get_range_status(&ready_right);
00226             board->sensor_right->vl53l1x_get_distance(&distance_right);
00227           }  
00228           checktof =0;
00229           if (distance_left <= ToF_SOGLIA_CRITICA || distance_right <= ToF_SOGLIA_CRITICA) {
00230             led = 1;
00231             //printf("\r\n          DISTANZA PIU' PICCOLA DELLA SOGLIA !!!");
00232             //ThisThread::sleep_for(1000);
00233             stop_misura =1;
00234           } else {
00235             stop_misura =0;
00236           }
00237         }
00238         if (stop_misura) {
00239           printf("\r\n          DISTANZA PIU' PICCOLA DELLA SOGLIA !!!");    
00240         } else {
00241                  
00242           accelerometer->get_x_axes(axes);
00243           printf("LIS2DW12 [acc/mg]:  %6d, %6d, %6d\r\n", axes[0], axes[1], axes[2]);
00244 
00245           acc_gyro->get_x_axes(axes);
00246           printf("LSM6DSO [acc/mg]:      %6d, %6d, %6d\r\n", axes[0], axes[1], axes[2]);
00247 
00248           acc_gyro->get_g_axes(axes);
00249           printf("LSM6DSO [gyro/mdps]:   %6d, %6d, %6d\r\n", axes[0], axes[1], axes[2]);
00250           //ThisThread::sleep_for(1000);
00251      
00252             
00253             
00254         }
00255         #if DISPLAY_CENTRE
00256         if (board->sensor_centre != NULL) {
00257             printf("Distance read by centre sensor is : %d mm\r\n", distance_centre);
00258         }
00259         #endif
00260         #if DISPLAY_LEFT
00261         if (board->sensor_left != NULL) {
00262             printf("Distance read by left satellite sensor is : %d mm\r\n", distance_left);
00263         }
00264         #endif
00265         #if DISPLAY_RIGHT
00266         if (board->sensor_right != NULL) {
00267             printf("Distance read by right satellite sensor is : %d mm\r\n", distance_right);
00268         }
00269         #endif
00270 
00271     }
00272 
00273     return status;
00274 }