main.cpp

00001 /*
00002  * This VL53L1X satellite board sample application performs range measurements
00003  * with interrupts enabled to generate a hardware interrupt each time a new
00004  * measurement is ready to be read.
00005  *
00006  *  Measured ranges are output on the Serial Port, running at 115200 baud.
00007  *
00008  *  On STM32-Nucleo boards :
00009  *      The User Blue button stops the current measurement and entire program,
00010  *      releasing all resources.
00011  *
00012  *      The Black Reset button is used to restart the program.
00013  *
00014  *  *** NOTE : By default hardlinks U10, U11, U15 & U18, on the underside of
00015  *            the X-NUCELO-53L1A1 expansion board are not made/OFF.
00016  *            These links must be made to allow interrupts from the Satellite boards
00017  *            to be received.
00018  *            U11 and U18 must be made/ON to allow interrupts to be received from the
00019  *            INT_L & INT_R positions; or
00020  *            U10 and U15 must be made/ON to allow interrupts to be received from the
00021  *            Alternate INT_L & INT_R positions.
00022  *            The X_NUCLEO_53L1A1 firmware library defaults to use the INT_L/INT_R
00023  *            positions.
00024  *            INT_L is available on expansion board Arduino Connector CN5, pin 1 as D9.
00025  *            Alternate INT_L is on CN5 Connector pin 2 as D8.
00026  *            INT_R is available on expansion board Arduino Connector CN9, pin 3 as D4.
00027  *            Alternate INT_R is on CN9 Connector pin 5 as D2.
00028  *            The pinouts are shown here : https://developer.mbed.org/components/X-NUCLEO-53L1A1/
00029  */
00030 
00031 #include <stdio.h>
00032 
00033 #include "mbed.h"
00034 #include "VL53L1X_I2C.h"
00035 #include "VL53L1X_Class.h"
00036 #include "LED_WS2812.h"
00037 
00038 #define VL53L1_I2C_SDA   D14
00039 #define VL53L1_I2C_SCL   D15
00040 
00041 static VL53L1X *sensor = NULL;
00042 Serial pc(SERIAL_TX, SERIAL_RX);
00043 PwmOut servo(D6);
00044 
00045 /* flags that handle interrupt request for sensor and user blue button*/
00046 volatile bool int_sensor = false;
00047 volatile bool int_stop = false;
00048 
00049 
00050 /* ISR callback function of the sensor */
00051 void sensor_irq(void)
00052 {
00053     int_sensor = true;
00054     sensor->disable_interrupt_measure_detection_irq();
00055 }
00056 
00057 /* ISR callback function of the user blue button to switch measuring sensor. */
00058 void measuring_stop_irq(void)
00059 {
00060     int_stop = true;
00061 }
00062 
00063 /* Start the sensor ranging */
00064 int start_ranging()
00065 {
00066     int status = 0;
00067     /* start the measure on the sensor */
00068     if (NULL != sensor) {
00069         status = sensor->stop_measurement();
00070         if (status != 0) {
00071                 return status;
00072         }
00073 
00074         status = sensor->start_measurement(&sensor_irq);
00075         if (status != 0) {
00076             return status;
00077         }
00078     }
00079     return status;
00080 }
00081 
00082 int range_measure(VL53L1X_DevI2C *device_i2c)
00083 {
00084     int status = 0;
00085     uint16_t distance = 0;
00086     int distance_close = 0;
00087     
00088     LED_WS2812 LED(A0,12);
00089     LED.SetIntensity(5);
00090     LED.SetColor(BLACK);
00091     
00092     /* Create a xshutdown pin */
00093     DigitalOut xshutdown(D7);
00094 
00095     /* create instance of sensor class */
00096     sensor = new VL53L1X(device_i2c, &xshutdown, A2);
00097 
00098     sensor->vl53l1_off();
00099     /* initialise sensor */
00100     sensor->init_sensor(0x52);
00101 
00102     if (status) {
00103         delete sensor;
00104         sensor= NULL;
00105         printf("Sensor centre not present\n\r");
00106     }
00107 
00108     status = sensor->vl53l1x_set_inter_measurement_in_ms(1000);    
00109     status = sensor->vl53l1x_set_distance_threshold(50,
00110                   50, 0, 0);
00111     status = start_ranging();
00112     if (status != 0) {
00113         printf("Failed to start ranging!\r\n");
00114         return status;
00115     }
00116     if (NULL != sensor) {
00117         printf("Entering loop mode\r\n");
00118         /* Main ranging interrupt loop */
00119         while (true) {
00120             if (int_sensor) {
00121                 
00122                 servo.period_ms(20);
00123                 
00124                 servo.pulsewidth_us(1150);
00125                 wait_ms(100);    
00126                 servo.pulsewidth_us(1100);
00127                 wait_ms(100);    
00128                 servo.pulsewidth_us(1050);
00129                 wait_ms(100);    
00130                 servo.pulsewidth_us(1000);
00131                 wait_ms(100);    
00132                 servo.pulsewidth_us(950);
00133                 wait_ms(100);    
00134                 servo.pulsewidth_us(900);
00135                 wait_ms(100);    
00136 
00137                 servo.pulsewidth_us(850);
00138                 wait_ms(100);    
00139                 servo.pulsewidth_us(800);
00140                 wait_ms(100);
00141                 servo.pulsewidth_us(750);
00142                 wait_ms(100);
00143                 servo.pulsewidth_us(700);
00144 //                wait_ms(100);
00145 //                servo.pulsewidth_us(650);
00146 //                wait_ms(100);
00147 //                servo.pulsewidth_us(600);
00148 //                wait_ms(100);   
00149 //                servo.pulsewidth_us(550);
00150 //                wait_ms(100);
00151 //                servo.pulsewidth_us(500);
00152  
00153                 wait_ms(500);
00154                 servo.pulsewidth_us(1150);
00155                 
00156                 int_sensor = false;
00157                 status = sensor->handle_irq(&distance);
00158                 printf("distance: %d\r\n", distance);         
00159             }
00160 
00161         }
00162     }
00163 
00164     return status;
00165 
00166 }
00167 
00168 
00169 /*=================================== Main ==================================
00170 =============================================================================*/
00171 int main()
00172 {
00173 #if TARGET_STM  // we are cross compiling for an STM32-Nucleo    
00174     InterruptIn stop_button(USER_BUTTON);
00175     stop_button.rise(&measuring_stop_irq);
00176 #endif
00177 #if TARGET_Freescale // we are cross-compiling for NXP FRDM boards.
00178     InterruptIn stop_button(SW2);
00179     stop_button.rise(&measuring_stop_irq);
00180 #endif
00181     VL53L1X_DevI2C *device_i2c = new VL53L1X_DevI2C(VL53L1_I2C_SDA, VL53L1_I2C_SCL);
00182     range_measure(device_i2c);  // start continuous measures
00183 }