main.cpp

00001 #include "mbed.h"
00002 #include "XNucleo53L0A1.h"
00003 #include "tof_gestures_DIRSWIPE_1.h"
00004 #include "tof_gestures.h"
00005 #include <string.h>
00006 #include <stdlib.h>
00007 #include <stdio.h>
00008 #include <assert.h>
00009 
00010 /*
00011  * This VL53L0X Expansion board sample application performs range measurements using
00012  * range_continuous_interrupt mode to generate a hardware interrupt each time a new
00013  * measurement is ready to be read.
00014  * The application supports the centre, on-board, sensor and up two satellite boards.
00015  *
00016  * The measured range data is displayed on the on-board 4-digit LED display.
00017  *
00018  * The User Blue button switches between the currently selected sensor to display range
00019  * results from.
00020  *
00021  * The Black Reset button is used to restart the program.
00022  *
00023  * *** NOTE : By default hardlinks U10, U11, U15 & U18, on the underside of
00024  *            the X-NUCELO-53L0A1 expansion board are not made/OFF.
00025  *            These links must be made to allow interrupts from the Satellite boards
00026  *            to be received.
00027  *            U11 and U18 must be made/ON to allow interrupts to be received from the
00028  *            INT_L & INT_R positions; or
00029  *            U10 and U15 must be made/ON to allow interrupts to be received from the
00030  *            Alternate INT_L & INT_R positions.
00031  *            The X_NUCLEO_53L0A1 firmware library defaults to use the INT_L/INT_R
00032  *            positions.
00033  *            INT_L is available on expansion board Arduino Connector CN5, pin 1 as D8.
00034  *            Alternate INT_L is on CN5 Connector pin 2 as D9.
00035  *            INT_R is available on expansion board Arduino Connector CN9, pin 3 as D2.
00036  *            Alternate INT_R is on CN9 Connector pin 5 as D4.
00037  *            The pinouts are shown here : https://developer.mbed.org/components/X-NUCLEO-53L0A1/
00038  *
00039  */
00040 
00041 #define VL53L0_I2C_SDA   D14
00042 #define VL53L0_I2C_SCL   D15
00043 
00044 #if USER_BUTTON==PC_13  // we are cross compiling for Nucleo-64s
00045 InterruptIn stop_button(USER_BUTTON);
00046 #endif
00047 
00048 static XNucleo53L0A1 *board = NULL;
00049 VL53L0X_RangingMeasurementData_t data_sensor;
00050 VL53L0X_RangingMeasurementData_t data_sensor_l;
00051 VL53L0X_RangingMeasurementData_t data_sensor_r;
00052 OperatingMode operating_mode;
00053 
00054 Gesture_DIRSWIPE_1_Data_t gestureDirSwipeData;
00055 
00056 /* current displayed sensor change IRQ */
00057 volatile bool switchChanged = false;
00058 
00059 /* interrupt requests */
00060 volatile bool centerSensor = false;
00061 volatile bool leftSensor = false;
00062 volatile bool rightSensor = false;
00063 
00064 /* Current sensor number*/
00065 volatile int currentSensor = 0;
00066 /* Installed sensors count */
00067 int sensorCnt = 0;
00068 
00069 /* installed sensors prefixes */
00070 char installedSensors[3];
00071 
00072 /* ISR callback function of the sensor_centre */
00073 void sensor_centre_irq(void)
00074 {
00075     centerSensor = true;
00076     board->sensor_centre->disable_interrupt_measure_detection_irq();
00077 }
00078 
00079 void sensor_left_irq(void)
00080 {
00081     leftSensor = true;
00082     board->sensor_left->disable_interrupt_measure_detection_irq();
00083 }
00084 
00085 void sensor_right_irq(void)
00086 {
00087     rightSensor = true;
00088     board->sensor_right->disable_interrupt_measure_detection_irq();
00089 }
00090 
00091 /* ISR callback function of the user blue button to switch measuring sensor. */
00092 void switch_measuring_sensor_irq(void)
00093 {
00094     stop_button.disable_irq();
00095     switchChanged = true;
00096 }
00097 
00098 
00099 /* On board 4 digit local display refresh */
00100 void refresh_display(const VL53L0X_RangingMeasurementData_t &data, char prefix)
00101 {
00102     static char str[5];
00103     if (data_sensor.RangeStatus == 0) { // we have a valid range.
00104         sprintf(str, "%c%3d", prefix, data.RangeMilliMeter);
00105         board->display->display_string(str);
00106     } else {
00107         sprintf(str, "%c%s", prefix, "---");
00108         board->display->display_string(str);
00109     }
00110 }
00111 
00112 DigitalOut sl(PC_10);
00113 DigitalOut sr(PA_8);
00114 
00115 inline void measure_sensors(OperatingMode op_mode)
00116 {
00117     bool current = false;
00118     int32_t leftRange, rightRange;
00119     int gesture_code;
00120     int meas_ready_l, meas_ready_r;
00121     uint8_t measurement_data_ready=0;   
00122     VL53L0X_Error status;     
00123        
00124     meas_ready_r =0; meas_ready_l =0;
00125     data_sensor_l.RangeMilliMeter = 1200;
00126     data_sensor_r.RangeMilliMeter = 1200;        
00127     leftRange=1200;
00128     rightRange=1200;
00129     VL53L0X_DEV dev_l, dev_r;
00130     VL53L0X_RangingMeasurementData_t ranging_measurement_data_l;
00131     VL53L0X_RangingMeasurementData_t ranging_measurement_data_r;    
00132     
00133     board->sensor_left->vl53l0x_get_device(&dev_l);   
00134     board->sensor_right->vl53l0x_get_device(&dev_r);  
00135 
00136     sl=0;
00137     sr=0;   
00138     // start single range measure
00139     status = board->sensor_left->VL53L0X_start_measurement(dev_l);
00140     if (status != 0) printf ("ERROR line: %d\n\r",__LINE__);    
00141     status = board->sensor_right->VL53L0X_start_measurement(dev_r);    
00142     if (status != 0) printf ("ERROR line: %d\n\r",__LINE__);        
00143     
00144     for (;meas_ready_r==0 && meas_ready_l==0;) {
00145     
00146         status = board->sensor_left->VL53L0X_get_measurement_data_ready(dev_l, &measurement_data_ready);
00147         if (status != 0) printf ("ERROR line: %d\n\r",__LINE__);           
00148         if (measurement_data_ready ==1 && status ==0 ) {
00149             sl=1;            
00150             board->sensor_left->VL53L0X_get_ranging_measurement_data(dev_l, &ranging_measurement_data_l);  
00151             board->sensor_left->VL53L0X_clear_interrupt_mask(dev_l, 0);                    
00152 //            printf ("L = %d\n\r", ranging_measurement_data_l.RangeMilliMeter);
00153             meas_ready_l = 1;
00154             leftRange = ranging_measurement_data_l.RangeMilliMeter;  
00155             sl=0;
00156         } else {
00157             meas_ready_l = 0;
00158             leftRange = 1200;
00159             ranging_measurement_data_l.RangeMilliMeter = 1200;
00160         }
00161     
00162         status = board->sensor_right->VL53L0X_get_measurement_data_ready(dev_r, &measurement_data_ready);
00163         if (status != 0) printf ("ERROR line: %d\n\r",__LINE__);           
00164         if (measurement_data_ready ==1 && status ==0 ) {
00165             sr=1;            
00166             board->sensor_right->VL53L0X_get_ranging_measurement_data(dev_r, &ranging_measurement_data_r); 
00167             board->sensor_right->VL53L0X_clear_interrupt_mask(dev_r, 0);                                
00168 //            printf ("R = %d\n\r", ranging_measurement_data_r.RangeMilliMeter);
00169             meas_ready_r = 1;
00170             rightRange = ranging_measurement_data_r.RangeMilliMeter;           
00171             sr=0;  
00172         } else {
00173            meas_ready_r = 0;
00174            rightRange = 1200;
00175            data_sensor_r.RangeMilliMeter = 1200;
00176         }        
00177     }
00178                
00179     gesture_code = tof_gestures_detectDIRSWIPE_1(leftRange, rightRange, &gestureDirSwipeData);
00180 
00181     /* Format data to display */
00182     if(gesture_code == GESTURES_SWIPE_LEFT_RIGHT){
00183         printf ("right --->>>\n\r");
00184     }else if(gesture_code == GESTURES_SWIPE_RIGHT_LEFT){
00185         printf ("<<<---\n\r");                    
00186     }else{
00187         //ShowGestureHelpMsg(gesture_code);
00188     }    
00189 }
00190 
00191 int init_sensors_array()
00192 {
00193     int status = 1;
00194     sensorCnt = 0;
00195     /* start the measure on the center sensor */
00196     if (NULL != board->sensor_centre) {
00197         installedSensors[sensorCnt] = 'C';
00198         status = board->sensor_centre->stop_measurement(operating_mode);
00199 //        status = board->sensor_centre->start_measurement(operating_mode, &sensor_centre_irq);
00200         ++sensorCnt;
00201     }
00202     /* start the measure on the left sensor */
00203     if (NULL != board->sensor_left) {
00204         installedSensors[sensorCnt] = 'L';        
00205         VL53L0X_DEV dev_l;
00206         board->sensor_left->vl53l0x_get_device(&dev_l);            
00207         uint8_t vhv_settings, phase_cal;
00208         board->sensor_left->VL53L0X_perform_ref_calibration(dev_l, &vhv_settings, &phase_cal);
00209         board->sensor_left->VL53L0X_set_measurement_timing_budget_micro_seconds(dev_l, 20000);
00210         ++sensorCnt;
00211     }
00212     
00213     /* start the measure on the right sensor */
00214     if (NULL != board->sensor_right) {
00215         installedSensors[sensorCnt] = 'R';
00216         VL53L0X_DEV dev_r;
00217         board->sensor_right->vl53l0x_get_device(&dev_r);            
00218         uint8_t vhv_settings, phase_cal;
00219         board->sensor_right->VL53L0X_perform_ref_calibration(dev_r, &vhv_settings, &phase_cal);
00220         board->sensor_right->VL53L0X_set_measurement_timing_budget_micro_seconds(dev_r, 20000);
00221         ++sensorCnt;
00222     }
00223     currentSensor = 0;
00224     return status;
00225 }
00226 
00227 
00228 void range_measure(DevI2C *device_i2c)
00229 {
00230     int status;
00231 
00232     /* creates the 53L0A1 expansion board singleton obj */
00233     board = XNucleo53L0A1::instance(device_i2c, A2, D8, D2);
00234     //board=X_NUCLEO_53L0A1::instance(device_i2c, A2, D9, D4); // Alternate INT_L/INT_R settings.
00235 
00236     board->display->display_string("53L0");
00237 
00238 //    operating_mode = range_continuous_interrupt;
00239     operating_mode = range_single_shot_polling;
00240 
00241     /* init the 53L0A1 expansion board with default values */
00242     status = board->init_board();
00243 
00244     if (status) {
00245         printf("Failed to init board!\n\r");
00246     } else {
00247         status = init_sensors_array();
00248     }
00249   /* Initialize directional swipes recognition : swipe detected below 400 mm, no max speed, min duration is 1 sec for a swipe and hand must not cover both devices */
00250     tof_gestures_initDIRSWIPE_1(400, 0, 1000, false, &gestureDirSwipeData);
00251   /* Select which module to debug (code must be compiled with TRACE defined in compiler command line) */
00252   TOF_GESTURES_DEBUG_SET_MODULES(NONE); // Could be NONE or TAP_1|TAP_SWIPE_2|DIRSWIPE_1 for instance (or any other combinations);
00253   
00254     if (!status) {
00255         printf("\r\nEntering loop mode\r\n");
00256         while (true) {
00257             measure_sensors(operating_mode);
00258             if (switchChanged) {
00259                 ++currentSensor;
00260                 if (currentSensor == sensorCnt)
00261                     currentSensor = 0;
00262 
00263                 printf("Sensor changed to %c\r\n", installedSensors[currentSensor]);
00264                 switchChanged = false;
00265                 stop_button.enable_irq();
00266             }            
00267         }
00268     }
00269     delete board;
00270 }
00271 
00272 /*=================================== Main ==================================
00273  Press the blue user button to switch the displayed sensor.
00274 =============================================================================*/
00275 int main()
00276 {
00277 #if USER_BUTTON==PC_13  // we are cross compiling for Nucleo-f401 
00278     stop_button.rise(&switch_measuring_sensor_irq);
00279     stop_button.enable_irq();
00280 #endif
00281     DevI2C *device_i2c = new DevI2C(VL53L0_I2C_SDA, VL53L0_I2C_SCL);
00282     range_measure(device_i2c);  // start continuous measures
00283 }