ST / Mbed 2 deprecated 53L0A1_HandGestureRecognition

Hand Gesture recognition (L-R swipe) application based on L-R sensors.

Dependencies:   X_NUCLEO_53L0A1 mbed

Fork of 53L0A1_HandGestureRecognition by Licio Mapelli

main.cpp@8:8151f9abad18, 2017-12-18 (annotated)

Committer:
mapellil
Date:
Mon Dec 18 09:59:00 2017 +0000
Revision:
8:8151f9abad18
Parent:
7:d79cbeda2982 
Updated libs

Who changed what in which revision?

UserRevisionLine numberNew contents of line
johnAlexander 0:cd8e818d4288 1 #include "mbed.h"
mapellil 8:8151f9abad18 2 #include "XNucleo53L0A1.h"
mapellil 7:d79cbeda2982 3 #include "tof_gestures_DIRSWIPE_1.h"
mapellil 7:d79cbeda2982 4 #include "tof_gestures.h"
johnAlexander 2:bdf097d93eca 5 #include <string.h>
johnAlexander 2:bdf097d93eca 6 #include <stdlib.h>
johnAlexander 2:bdf097d93eca 7 #include <stdio.h>
johnAlexander 2:bdf097d93eca 8 #include <assert.h>
johnAlexander 2:bdf097d93eca 9
johnAlexander 5:906aa7aede10 10 /*
johnAlexander 5:906aa7aede10 11 * This VL53L0X Expansion board sample application performs range measurements using
johnAlexander 5:906aa7aede10 12 * range_continuous_interrupt mode to generate a hardware interrupt each time a new
johnAlexander 3:12cb106044f9 13 * measurement is ready to be read.
johnAlexander 4:44629a30d6f4 14 * The application supports the centre, on-board, sensor and up two satellite boards.
johnAlexander 3:12cb106044f9 15 *
johnAlexander 4:44629a30d6f4 16 * The measured range data is displayed on the on-board 4-digit LED display.
johnAlexander 3:12cb106044f9 17 *
johnAlexander 5:906aa7aede10 18 * The User Blue button switches between the currently selected sensor to display range
johnAlexander 3:12cb106044f9 19 * results from.
johnAlexander 3:12cb106044f9 20 *
johnAlexander 5:906aa7aede10 21 * The Black Reset button is used to restart the program.
johnAlexander 4:44629a30d6f4 22 *
johnAlexander 5:906aa7aede10 23 * *** NOTE : By default hardlinks U10, U11, U15 & U18, on the underside of
johnAlexander 4:44629a30d6f4 24 * the X-NUCELO-53L0A1 expansion board are not made/OFF.
johnAlexander 5:906aa7aede10 25 * These links must be made to allow interrupts from the Satellite boards
johnAlexander 4:44629a30d6f4 26 * to be received.
johnAlexander 4:44629a30d6f4 27 * U11 and U18 must be made/ON to allow interrupts to be received from the
johnAlexander 4:44629a30d6f4 28 * INT_L & INT_R positions; or
johnAlexander 4:44629a30d6f4 29 * U10 and U15 must be made/ON to allow interrupts to be received from the
johnAlexander 5:906aa7aede10 30 * Alternate INT_L & INT_R positions.
johnAlexander 5:906aa7aede10 31 * The X_NUCLEO_53L0A1 firmware library defaults to use the INT_L/INT_R
johnAlexander 5:906aa7aede10 32 * positions.
johnAlexander 4:44629a30d6f4 33 * INT_L is available on expansion board Arduino Connector CN5, pin 1 as D8.
johnAlexander 4:44629a30d6f4 34 * Alternate INT_L is on CN5 Connector pin 2 as D9.
johnAlexander 4:44629a30d6f4 35 * INT_R is available on expansion board Arduino Connector CN9, pin 3 as D2.
johnAlexander 5:906aa7aede10 36 * Alternate INT_R is on CN9 Connector pin 5 as D4.
johnAlexander 4:44629a30d6f4 37 * The pinouts are shown here : https://developer.mbed.org/components/X-NUCLEO-53L0A1/
johnAlexander 5:906aa7aede10 38 *
johnAlexander 3:12cb106044f9 39 */
johnAlexander 2:bdf097d93eca 40
johnAlexander 5:906aa7aede10 41 #define VL53L0_I2C_SDA D14
johnAlexander 5:906aa7aede10 42 #define VL53L0_I2C_SCL D15
johnAlexander 0:cd8e818d4288 43
johnAlexander 4:44629a30d6f4 44 #if USER_BUTTON==PC_13 // we are cross compiling for Nucleo-64s
johnAlexander 5:906aa7aede10 45 InterruptIn stop_button(USER_BUTTON);
johnAlexander 5:906aa7aede10 46 #endif
johnAlexander 3:12cb106044f9 47
mapellil 8:8151f9abad18 48 static XNucleo53L0A1 *board = NULL;
johnAlexander 2:bdf097d93eca 49 VL53L0X_RangingMeasurementData_t data_sensor;
mapellil 7:d79cbeda2982 50 VL53L0X_RangingMeasurementData_t data_sensor_l;
mapellil 7:d79cbeda2982 51 VL53L0X_RangingMeasurementData_t data_sensor_r;
johnAlexander 0:cd8e818d4288 52 OperatingMode operating_mode;
johnAlexander 4:44629a30d6f4 53
mapellil 7:d79cbeda2982 54 Gesture_DIRSWIPE_1_Data_t gestureDirSwipeData;
mapellil 7:d79cbeda2982 55
johnAlexander 4:44629a30d6f4 56 /* current displayed sensor change IRQ */
johnAlexander 4:44629a30d6f4 57 volatile bool switchChanged = false;
johnAlexander 4:44629a30d6f4 58
johnAlexander 3:12cb106044f9 59 /* interrupt requests */
johnAlexander 4:44629a30d6f4 60 volatile bool centerSensor = false;
johnAlexander 4:44629a30d6f4 61 volatile bool leftSensor = false;
johnAlexander 5:906aa7aede10 62 volatile bool rightSensor = false;
johnAlexander 3:12cb106044f9 63
johnAlexander 3:12cb106044f9 64 /* Current sensor number*/
johnAlexander 3:12cb106044f9 65 volatile int currentSensor = 0;
johnAlexander 5:906aa7aede10 66 /* Installed sensors count */
johnAlexander 5:906aa7aede10 67 int sensorCnt = 0;
johnAlexander 3:12cb106044f9 68
johnAlexander 4:44629a30d6f4 69 /* installed sensors prefixes */
johnAlexander 4:44629a30d6f4 70 char installedSensors[3];
johnAlexander 0:cd8e818d4288 71
johnAlexander 3:12cb106044f9 72 /* ISR callback function of the sensor_centre */
johnAlexander 5:906aa7aede10 73 void sensor_centre_irq(void)
johnAlexander 0:cd8e818d4288 74 {
johnAlexander 5:906aa7aede10 75 centerSensor = true;
johnAlexander 5:906aa7aede10 76 board->sensor_centre->disable_interrupt_measure_detection_irq();
johnAlexander 5:906aa7aede10 77 }
johnAlexander 2:bdf097d93eca 78
johnAlexander 5:906aa7aede10 79 void sensor_left_irq(void)
johnAlexander 3:12cb106044f9 80 {
johnAlexander 5:906aa7aede10 81 leftSensor = true;
johnAlexander 5:906aa7aede10 82 board->sensor_left->disable_interrupt_measure_detection_irq();
johnAlexander 5:906aa7aede10 83 }
johnAlexander 3:12cb106044f9 84
johnAlexander 5:906aa7aede10 85 void sensor_right_irq(void)
johnAlexander 5:906aa7aede10 86 {
johnAlexander 5:906aa7aede10 87 rightSensor = true;
johnAlexander 5:906aa7aede10 88 board->sensor_right->disable_interrupt_measure_detection_irq();
johnAlexander 5:906aa7aede10 89 }
johnAlexander 3:12cb106044f9 90
johnAlexander 2:bdf097d93eca 91 /* ISR callback function of the user blue button to switch measuring sensor. */
johnAlexander 5:906aa7aede10 92 void switch_measuring_sensor_irq(void)
johnAlexander 2:bdf097d93eca 93 {
johnAlexander 4:44629a30d6f4 94 stop_button.disable_irq();
johnAlexander 4:44629a30d6f4 95 switchChanged = true;
johnAlexander 0:cd8e818d4288 96 }
johnAlexander 0:cd8e818d4288 97
johnAlexander 4:44629a30d6f4 98
johnAlexander 0:cd8e818d4288 99 /* On board 4 digit local display refresh */
johnAlexander 5:906aa7aede10 100 void refresh_display(const VL53L0X_RangingMeasurementData_t &data, char prefix)
johnAlexander 4:44629a30d6f4 101 {
johnAlexander 4:44629a30d6f4 102 static char str[5];
johnAlexander 5:906aa7aede10 103 if (data_sensor.RangeStatus == 0) { // we have a valid range.
johnAlexander 5:906aa7aede10 104 sprintf(str, "%c%3d", prefix, data.RangeMilliMeter);
johnAlexander 5:906aa7aede10 105 board->display->display_string(str);
johnAlexander 5:906aa7aede10 106 } else {
johnAlexander 4:44629a30d6f4 107 sprintf(str, "%c%s", prefix, "---");
johnAlexander 5:906aa7aede10 108 board->display->display_string(str);
johnAlexander 4:44629a30d6f4 109 }
johnAlexander 4:44629a30d6f4 110 }
johnAlexander 4:44629a30d6f4 111
mapellil 7:d79cbeda2982 112 DigitalOut sl(PC_10);
mapellil 7:d79cbeda2982 113 DigitalOut sr(PA_8);
mapellil 7:d79cbeda2982 114
johnAlexander 5:906aa7aede10 115 inline void measure_sensors(OperatingMode op_mode)
johnAlexander 4:44629a30d6f4 116 {
johnAlexander 4:44629a30d6f4 117 bool current = false;
mapellil 7:d79cbeda2982 118 int32_t leftRange, rightRange;
mapellil 7:d79cbeda2982 119 int gesture_code;
mapellil 7:d79cbeda2982 120 int meas_ready_l, meas_ready_r;
mapellil 7:d79cbeda2982 121 uint8_t measurement_data_ready=0;
mapellil 7:d79cbeda2982 122 VL53L0X_Error status;
mapellil 7:d79cbeda2982 123
mapellil 7:d79cbeda2982 124 meas_ready_r =0; meas_ready_l =0;
mapellil 7:d79cbeda2982 125 data_sensor_l.RangeMilliMeter = 1200;
mapellil 7:d79cbeda2982 126 data_sensor_r.RangeMilliMeter = 1200;
mapellil 7:d79cbeda2982 127 leftRange=1200;
mapellil 7:d79cbeda2982 128 rightRange=1200;
mapellil 7:d79cbeda2982 129 VL53L0X_DEV dev_l, dev_r;
mapellil 7:d79cbeda2982 130 VL53L0X_RangingMeasurementData_t ranging_measurement_data_l;
mapellil 7:d79cbeda2982 131 VL53L0X_RangingMeasurementData_t ranging_measurement_data_r;
mapellil 7:d79cbeda2982 132
mapellil 7:d79cbeda2982 133 board->sensor_left->vl53l0x_get_device(&dev_l);
mapellil 7:d79cbeda2982 134 board->sensor_right->vl53l0x_get_device(&dev_r);
mapellil 7:d79cbeda2982 135
mapellil 7:d79cbeda2982 136 sl=0;
mapellil 7:d79cbeda2982 137 sr=0;
mapellil 7:d79cbeda2982 138 // start single range measure
mapellil 8:8151f9abad18 139 status = board->sensor_left->VL53L0X_start_measurement(dev_l);
mapellil 7:d79cbeda2982 140 if (status != 0) printf ("ERROR line: %d\n\r",__LINE__);
mapellil 8:8151f9abad18 141 status = board->sensor_right->VL53L0X_start_measurement(dev_r);
mapellil 7:d79cbeda2982 142 if (status != 0) printf ("ERROR line: %d\n\r",__LINE__);
mapellil 7:d79cbeda2982 143
mapellil 7:d79cbeda2982 144 for (;meas_ready_r==0 && meas_ready_l==0;) {
mapellil 7:d79cbeda2982 145
mapellil 8:8151f9abad18 146 status = board->sensor_left->VL53L0X_get_measurement_data_ready(dev_l, &measurement_data_ready);
mapellil 7:d79cbeda2982 147 if (status != 0) printf ("ERROR line: %d\n\r",__LINE__);
mapellil 7:d79cbeda2982 148 if (measurement_data_ready ==1 && status ==0 ) {
mapellil 7:d79cbeda2982 149 sl=1;
mapellil 8:8151f9abad18 150 board->sensor_left->VL53L0X_get_ranging_measurement_data(dev_l, &ranging_measurement_data_l);
mapellil 8:8151f9abad18 151 board->sensor_left->VL53L0X_clear_interrupt_mask(dev_l, 0);
mapellil 7:d79cbeda2982 152 // printf ("L = %d\n\r", ranging_measurement_data_l.RangeMilliMeter);
mapellil 7:d79cbeda2982 153 meas_ready_l = 1;
mapellil 7:d79cbeda2982 154 leftRange = ranging_measurement_data_l.RangeMilliMeter;
mapellil 7:d79cbeda2982 155 sl=0;
mapellil 7:d79cbeda2982 156 } else {
mapellil 7:d79cbeda2982 157 meas_ready_l = 0;
mapellil 7:d79cbeda2982 158 leftRange = 1200;
mapellil 7:d79cbeda2982 159 ranging_measurement_data_l.RangeMilliMeter = 1200;
johnAlexander 3:12cb106044f9 160 }
mapellil 7:d79cbeda2982 161
mapellil 8:8151f9abad18 162 status = board->sensor_right->VL53L0X_get_measurement_data_ready(dev_r, &measurement_data_ready);
mapellil 7:d79cbeda2982 163 if (status != 0) printf ("ERROR line: %d\n\r",__LINE__);
mapellil 7:d79cbeda2982 164 if (measurement_data_ready ==1 && status ==0 ) {
mapellil 7:d79cbeda2982 165 sr=1;
mapellil 8:8151f9abad18 166 board->sensor_right->VL53L0X_get_ranging_measurement_data(dev_r, &ranging_measurement_data_r);
mapellil 8:8151f9abad18 167 board->sensor_right->VL53L0X_clear_interrupt_mask(dev_r, 0);
mapellil 7:d79cbeda2982 168 // printf ("R = %d\n\r", ranging_measurement_data_r.RangeMilliMeter);
mapellil 7:d79cbeda2982 169 meas_ready_r = 1;
mapellil 7:d79cbeda2982 170 rightRange = ranging_measurement_data_r.RangeMilliMeter;
mapellil 7:d79cbeda2982 171 sr=0;
mapellil 7:d79cbeda2982 172 } else {
mapellil 7:d79cbeda2982 173 meas_ready_r = 0;
mapellil 7:d79cbeda2982 174 rightRange = 1200;
mapellil 7:d79cbeda2982 175 data_sensor_r.RangeMilliMeter = 1200;
mapellil 7:d79cbeda2982 176 }
johnAlexander 2:bdf097d93eca 177 }
mapellil 7:d79cbeda2982 178
mapellil 7:d79cbeda2982 179 gesture_code = tof_gestures_detectDIRSWIPE_1(leftRange, rightRange, &gestureDirSwipeData);
mapellil 7:d79cbeda2982 180
mapellil 7:d79cbeda2982 181 /* Format data to display */
mapellil 7:d79cbeda2982 182 if(gesture_code == GESTURES_SWIPE_LEFT_RIGHT){
mapellil 7:d79cbeda2982 183 printf ("--->>>\n\r");
mapellil 7:d79cbeda2982 184 }else if(gesture_code == GESTURES_SWIPE_RIGHT_LEFT){
mapellil 7:d79cbeda2982 185 printf ("<<<---\n\r");
mapellil 7:d79cbeda2982 186 }else{
mapellil 7:d79cbeda2982 187 //ShowGestureHelpMsg(gesture_code);
mapellil 7:d79cbeda2982 188 }
johnAlexander 3:12cb106044f9 189 }
johnAlexander 3:12cb106044f9 190
johnAlexander 5:906aa7aede10 191 int init_sensors_array()
johnAlexander 3:12cb106044f9 192 {
johnAlexander 3:12cb106044f9 193 int status = 1;
johnAlexander 3:12cb106044f9 194 sensorCnt = 0;
johnAlexander 3:12cb106044f9 195 /* start the measure on the center sensor */
johnAlexander 5:906aa7aede10 196 if (NULL != board->sensor_centre) {
johnAlexander 4:44629a30d6f4 197 installedSensors[sensorCnt] = 'C';
johnAlexander 5:906aa7aede10 198 status = board->sensor_centre->stop_measurement(operating_mode);
mapellil 7:d79cbeda2982 199 // status = board->sensor_centre->start_measurement(operating_mode, &sensor_centre_irq);
johnAlexander 3:12cb106044f9 200 ++sensorCnt;
johnAlexander 3:12cb106044f9 201 }
johnAlexander 3:12cb106044f9 202 /* start the measure on the left sensor */
johnAlexander 5:906aa7aede10 203 if (NULL != board->sensor_left) {
mapellil 7:d79cbeda2982 204 installedSensors[sensorCnt] = 'L';
mapellil 7:d79cbeda2982 205 VL53L0X_DEV dev_l;
mapellil 7:d79cbeda2982 206 board->sensor_left->vl53l0x_get_device(&dev_l);
mapellil 7:d79cbeda2982 207 uint8_t vhv_settings, phase_cal;
mapellil 8:8151f9abad18 208 board->sensor_left->VL53L0X_perform_ref_calibration(dev_l, &vhv_settings, &phase_cal);
mapellil 8:8151f9abad18 209 board->sensor_left->VL53L0X_set_measurement_timing_budget_micro_seconds(dev_l, 20000);
johnAlexander 3:12cb106044f9 210 ++sensorCnt;
johnAlexander 3:12cb106044f9 211 }
mapellil 7:d79cbeda2982 212
johnAlexander 5:906aa7aede10 213 /* start the measure on the right sensor */
johnAlexander 5:906aa7aede10 214 if (NULL != board->sensor_right) {
johnAlexander 4:44629a30d6f4 215 installedSensors[sensorCnt] = 'R';
mapellil 7:d79cbeda2982 216 VL53L0X_DEV dev_r;
mapellil 7:d79cbeda2982 217 board->sensor_right->vl53l0x_get_device(&dev_r);
mapellil 7:d79cbeda2982 218 uint8_t vhv_settings, phase_cal;
mapellil 8:8151f9abad18 219 board->sensor_right->VL53L0X_perform_ref_calibration(dev_r, &vhv_settings, &phase_cal);
mapellil 8:8151f9abad18 220 board->sensor_right->VL53L0X_set_measurement_timing_budget_micro_seconds(dev_r, 20000);
johnAlexander 3:12cb106044f9 221 ++sensorCnt;
johnAlexander 3:12cb106044f9 222 }
johnAlexander 3:12cb106044f9 223 currentSensor = 0;
johnAlexander 3:12cb106044f9 224 return status;
johnAlexander 0:cd8e818d4288 225 }
johnAlexander 0:cd8e818d4288 226
johnAlexander 4:44629a30d6f4 227
johnAlexander 5:906aa7aede10 228 void range_measure(DevI2C *device_i2c)
johnAlexander 5:906aa7aede10 229 {
johnAlexander 5:906aa7aede10 230 int status;
johnAlexander 5:906aa7aede10 231
johnAlexander 5:906aa7aede10 232 /* creates the 53L0A1 expansion board singleton obj */
mapellil 8:8151f9abad18 233 board = XNucleo53L0A1::instance(device_i2c, A2, D8, D2);
johnAlexander 5:906aa7aede10 234 //board=X_NUCLEO_53L0A1::instance(device_i2c, A2, D9, D4); // Alternate INT_L/INT_R settings.
johnAlexander 5:906aa7aede10 235
johnAlexander 5:906aa7aede10 236 board->display->display_string("53L0");
johnAlexander 5:906aa7aede10 237
mapellil 7:d79cbeda2982 238 // operating_mode = range_continuous_interrupt;
mapellil 7:d79cbeda2982 239 operating_mode = range_single_shot_polling;
johnAlexander 5:906aa7aede10 240
johnAlexander 5:906aa7aede10 241 /* init the 53L0A1 expansion board with default values */
johnAlexander 5:906aa7aede10 242 status = board->init_board();
johnAlexander 1:c1c10e0c32b2 243
johnAlexander 5:906aa7aede10 244 if (status) {
johnAlexander 5:906aa7aede10 245 printf("Failed to init board!\n\r");
johnAlexander 5:906aa7aede10 246 } else {
johnAlexander 5:906aa7aede10 247 status = init_sensors_array();
johnAlexander 5:906aa7aede10 248 }
mapellil 7:d79cbeda2982 249 /* 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 */
mapellil 7:d79cbeda2982 250 tof_gestures_initDIRSWIPE_1(400, 0, 1000, false, &gestureDirSwipeData);
mapellil 7:d79cbeda2982 251 /* Select which module to debug (code must be compiled with TRACE defined in compiler command line) */
mapellil 7:d79cbeda2982 252 TOF_GESTURES_DEBUG_SET_MODULES(NONE); // Could be NONE or TAP_1|TAP_SWIPE_2|DIRSWIPE_1 for instance (or any other combinations);
mapellil 7:d79cbeda2982 253
johnAlexander 5:906aa7aede10 254 if (!status) {
johnAlexander 5:906aa7aede10 255 printf("\r\nEntering loop mode\r\n");
johnAlexander 5:906aa7aede10 256 while (true) {
johnAlexander 5:906aa7aede10 257 measure_sensors(operating_mode);
johnAlexander 5:906aa7aede10 258 if (switchChanged) {
johnAlexander 5:906aa7aede10 259 ++currentSensor;
johnAlexander 5:906aa7aede10 260 if (currentSensor == sensorCnt)
johnAlexander 5:906aa7aede10 261 currentSensor = 0;
johnAlexander 5:906aa7aede10 262
johnAlexander 5:906aa7aede10 263 printf("Sensor changed to %c\r\n", installedSensors[currentSensor]);
johnAlexander 5:906aa7aede10 264 switchChanged = false;
johnAlexander 5:906aa7aede10 265 stop_button.enable_irq();
mapellil 7:d79cbeda2982 266 }
johnAlexander 4:44629a30d6f4 267 }
johnAlexander 5:906aa7aede10 268 }
johnAlexander 5:906aa7aede10 269 delete board;
johnAlexander 5:906aa7aede10 270 }
johnAlexander 2:bdf097d93eca 271
johnAlexander 2:bdf097d93eca 272 /*=================================== Main ==================================
johnAlexander 5:906aa7aede10 273 Press the blue user button to switch the displayed sensor.
johnAlexander 2:bdf097d93eca 274 =============================================================================*/
johnAlexander 2:bdf097d93eca 275 int main()
johnAlexander 5:906aa7aede10 276 {
johnAlexander 2:bdf097d93eca 277 #if USER_BUTTON==PC_13 // we are cross compiling for Nucleo-f401
johnAlexander 5:906aa7aede10 278 stop_button.rise(&switch_measuring_sensor_irq);
johnAlexander 5:906aa7aede10 279 stop_button.enable_irq();
johnAlexander 5:906aa7aede10 280 #endif
johnAlexander 5:906aa7aede10 281 DevI2C *device_i2c = new DevI2C(VL53L0_I2C_SDA, VL53L0_I2C_SCL);
johnAlexander 5:906aa7aede10 282 range_measure(device_i2c); // start continuous measures
johnAlexander 0:cd8e818d4288 283 }