Diff: main.cpp

Revision:

7:d79cbeda2982

Parent:

5:906aa7aede10

Child:

8:8151f9abad18

--- a/main.cpp	Tue Aug 08 14:45:08 2017 +0000
+++ b/main.cpp	Fri Dec 15 14:16:16 2017 +0000
@@ -1,5 +1,7 @@
 #include "mbed.h"
 #include "x_nucleo_53l0a1.h"
+#include "tof_gestures_DIRSWIPE_1.h"
+#include "tof_gestures.h"
 #include <string.h>
 #include <stdlib.h>
 #include <stdio.h>
@@ -45,8 +47,12 @@
 
 static X_NUCLEO_53L0A1 *board = NULL;
 VL53L0X_RangingMeasurementData_t data_sensor;
+VL53L0X_RangingMeasurementData_t data_sensor_l;
+VL53L0X_RangingMeasurementData_t data_sensor_r;
 OperatingMode operating_mode;
 
+Gesture_DIRSWIPE_1_Data_t gestureDirSwipeData;
+
 /* current displayed sensor change IRQ */
 volatile bool switchChanged = false;
 
@@ -103,33 +109,83 @@
     }
 }
 
+DigitalOut sl(PC_10);
+DigitalOut sr(PA_8);
+
 inline void measure_sensors(OperatingMode op_mode)
 {
     bool current = false;
-    if (centerSensor) {
-        centerSensor = false;
-        board->sensor_centre->handle_irq(op_mode, &data_sensor);
-        current = (currentSensor == 0);
-        if (current) {
-            refresh_display(data_sensor, 'C');
+    int32_t leftRange, rightRange;
+    int gesture_code;
+    int meas_ready_l, meas_ready_r;
+    uint8_t measurement_data_ready=0;   
+    VL53L0X_Error status;     
+       
+    meas_ready_r =0; meas_ready_l =0;
+    data_sensor_l.RangeMilliMeter = 1200;
+    data_sensor_r.RangeMilliMeter = 1200;        
+    leftRange=1200;
+    rightRange=1200;
+    VL53L0X_DEV dev_l, dev_r;
+    VL53L0X_RangingMeasurementData_t ranging_measurement_data_l;
+    VL53L0X_RangingMeasurementData_t ranging_measurement_data_r;    
+    
+    board->sensor_left->vl53l0x_get_device(&dev_l);   
+    board->sensor_right->vl53l0x_get_device(&dev_r);  
+
+    sl=0;
+    sr=0;   
+    // start single range measure
+    status = board->sensor_left->vl53l0x_start_measurement(dev_l);
+    if (status != 0) printf ("ERROR line: %d\n\r",__LINE__);    
+    status = board->sensor_right->vl53l0x_start_measurement(dev_r);    
+    if (status != 0) printf ("ERROR line: %d\n\r",__LINE__);        
+    
+    for (;meas_ready_r==0 && meas_ready_l==0;) {
+    
+        status = board->sensor_left->vl53l0x_get_measurement_data_ready(dev_l, &measurement_data_ready);
+        if (status != 0) printf ("ERROR line: %d\n\r",__LINE__);           
+        if (measurement_data_ready ==1 && status ==0 ) {
+            sl=1;            
+            board->sensor_left->vl53l0x_get_ranging_measurement_data(dev_l, &ranging_measurement_data_l);  
+            board->sensor_left->vl53l0x_clear_interrupt_mask(dev_l, 0);                    
+//            printf ("L = %d\n\r", ranging_measurement_data_l.RangeMilliMeter);
+            meas_ready_l = 1;
+            leftRange = ranging_measurement_data_l.RangeMilliMeter;  
+            sl=0;
+        } else {
+            meas_ready_l = 0;
+            leftRange = 1200;
+            ranging_measurement_data_l.RangeMilliMeter = 1200;
         }
+    
+        status = board->sensor_right->vl53l0x_get_measurement_data_ready(dev_r, &measurement_data_ready);
+        if (status != 0) printf ("ERROR line: %d\n\r",__LINE__);           
+        if (measurement_data_ready ==1 && status ==0 ) {
+            sr=1;            
+            board->sensor_right->vl53l0x_get_ranging_measurement_data(dev_r, &ranging_measurement_data_r); 
+            board->sensor_right->vl53l0x_clear_interrupt_mask(dev_r, 0);                                
+//            printf ("R = %d\n\r", ranging_measurement_data_r.RangeMilliMeter);
+            meas_ready_r = 1;
+            rightRange = ranging_measurement_data_r.RangeMilliMeter;           
+            sr=0;  
+        } else {
+           meas_ready_r = 0;
+           rightRange = 1200;
+           data_sensor_r.RangeMilliMeter = 1200;
+        }        
     }
-    if (leftSensor) {
-        leftSensor = false;
-        board->sensor_left->handle_irq(op_mode, &data_sensor);
-        current = (installedSensors[currentSensor] == 'L');
-        if (current) {
-            refresh_display(data_sensor, 'L');
-        }
-    }
-    if (rightSensor) {
-        rightSensor = false;
-        board->sensor_right->handle_irq(op_mode, &data_sensor);
-        current = (installedSensors[currentSensor] == 'R');
-        if (current) {
-            refresh_display(data_sensor, 'R');
-        }
-    }
+               
+    gesture_code = tof_gestures_detectDIRSWIPE_1(leftRange, rightRange, &gestureDirSwipeData);
+
+    /* Format data to display */
+    if(gesture_code == GESTURES_SWIPE_LEFT_RIGHT){
+        printf ("--->>>\n\r");
+    }else if(gesture_code == GESTURES_SWIPE_RIGHT_LEFT){
+        printf ("<<<---\n\r");                    
+    }else{
+        //ShowGestureHelpMsg(gesture_code);
+    }    
 }
 
 int init_sensors_array()
@@ -140,21 +196,28 @@
     if (NULL != board->sensor_centre) {
         installedSensors[sensorCnt] = 'C';
         status = board->sensor_centre->stop_measurement(operating_mode);
-        status = board->sensor_centre->start_measurement(operating_mode, &sensor_centre_irq);
+//        status = board->sensor_centre->start_measurement(operating_mode, &sensor_centre_irq);
         ++sensorCnt;
     }
     /* start the measure on the left sensor */
     if (NULL != board->sensor_left) {
-        installedSensors[sensorCnt] = 'L';
-        status = board->sensor_left->stop_measurement(operating_mode);
-        status = board->sensor_left->start_measurement(operating_mode, &sensor_left_irq);
+        installedSensors[sensorCnt] = 'L';        
+        VL53L0X_DEV dev_l;
+        board->sensor_left->vl53l0x_get_device(&dev_l);            
+        uint8_t vhv_settings, phase_cal;
+        board->sensor_left->vl53l0x_perform_ref_calibration(dev_l, &vhv_settings, &phase_cal);
+        board->sensor_left->vl53l0x_set_measurement_timing_budget_micro_seconds(dev_l, 20000);
         ++sensorCnt;
     }
+    
     /* start the measure on the right sensor */
     if (NULL != board->sensor_right) {
         installedSensors[sensorCnt] = 'R';
-        status = board->sensor_right->stop_measurement(operating_mode);
-        status = board->sensor_right->start_measurement(operating_mode, &sensor_right_irq);
+        VL53L0X_DEV dev_r;
+        board->sensor_right->vl53l0x_get_device(&dev_r);            
+        uint8_t vhv_settings, phase_cal;
+        board->sensor_right->vl53l0x_perform_ref_calibration(dev_r, &vhv_settings, &phase_cal);
+        board->sensor_right->vl53l0x_set_measurement_timing_budget_micro_seconds(dev_r, 20000);
         ++sensorCnt;
     }
     currentSensor = 0;
@@ -172,7 +235,8 @@
 
     board->display->display_string("53L0");
 
-    operating_mode = range_continuous_interrupt;
+//    operating_mode = range_continuous_interrupt;
+    operating_mode = range_single_shot_polling;
 
     /* init the 53L0A1 expansion board with default values */
     status = board->init_board();
@@ -182,7 +246,11 @@
     } else {
         status = init_sensors_array();
     }
-
+  /* 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 */
+    tof_gestures_initDIRSWIPE_1(400, 0, 1000, false, &gestureDirSwipeData);
+  /* Select which module to debug (code must be compiled with TRACE defined in compiler command line) */
+  TOF_GESTURES_DEBUG_SET_MODULES(NONE); // Could be NONE or TAP_1|TAP_SWIPE_2|DIRSWIPE_1 for instance (or any other combinations);
+  
     if (!status) {
         printf("\r\nEntering loop mode\r\n");
         while (true) {
@@ -195,7 +263,7 @@
                 printf("Sensor changed to %c\r\n", installedSensors[currentSensor]);
                 switchChanged = false;
                 stop_button.enable_irq();
-            }
+            }            
         }
     }
     delete board;
@@ -213,6 +281,3 @@
     DevI2C *device_i2c = new DevI2C(VL53L0_I2C_SDA, VL53L0_I2C_SCL);
     range_measure(device_i2c);  // start continuous measures
 }
-
-
-