main.cpp

00001 #include "mbed.h"
00002 #include "m3pi.h"
00003 #include "x_nucleo_53l0a1.h"
00004 #include <stdio.h>
00005 
00006 /* This VL53L0X Expansion board test application performs a range measurement in polling mode
00007    on the onboard embedded top sensor. */
00008 
00009 #define VL53L0_I2C_SDA   D14 
00010 #define VL53L0_I2C_SCL   D15 
00011 
00012 static X_NUCLEO_53L0A1 *board=NULL;
00013 
00014 
00015 //VIN -- VOUT
00016 //GND -- GND
00017 //SDA -- p9
00018 //SCA -- p10
00019 
00020 m3pi m3pi;
00021 DigitalOut led1(LED1);
00022 DigitalOut led2(LED2);
00023 
00024 int main() {
00025     //LIDAR setup
00026     int status;
00027     uint32_t distance;
00028     DevI2C *device_i2c =new DevI2C(p28, p27);         
00029     /* creates the 53L0A1 expansion board singleton obj */
00030     board = X_NUCLEO_53L0A1::Instance(device_i2c,p21,p21,p21);
00031     /* init the 53L0A1 expansion board with default values */
00032     status=board->InitBoard();
00033     
00034     if(status) { led1 = 1; return 0; }
00035     
00036     m3pi.locate(0,1);
00037     m3pi.printf("YO");
00038     
00039     while(1) {
00040         status = board->sensor_centre->GetDistance(&distance);
00041         if (status == VL53L0X_ERROR_NONE) {
00042             m3pi.locate(0,1);
00043             m3pi.printf("%u", distance);
00044             printf("%u\r\n", distance);
00045             if (distance > 50) {
00046                 m3pi.forward(.1);
00047                 led2 = 1;
00048             } else {
00049                 m3pi.stop();
00050                 led2 = 0;
00051             }
00052         }   
00053     }   
00054 }