Jenhao Ou / Mbed 2 deprecated lidarproj

Not working yet

Dependencies:   SG90 VL53L0X_simple mbed

main.cpp@0:8637a4213254, 2018-06-18 (annotated)

Committer:
oldmon
Date:
Mon Jun 18 12:03:09 2018 +0000
Revision:
0:8637a4213254
Child:
2:64a1dd5f6dc7
not okay with laser

Who changed what in which revision?

UserRevisionLine numberNew contents of line
oldmon 0:8637a4213254 1 #include "mbed.h"
oldmon 0:8637a4213254 2 #include "SG90.h"
oldmon 0:8637a4213254 3 #include "VL53L0X.h"
oldmon 0:8637a4213254 4 #define USE_XSHUT 0
oldmon 0:8637a4213254 5
oldmon 0:8637a4213254 6 DigitalIn Btn1(PC_13);// USER_BUTTON
oldmon 0:8637a4213254 7 SG90 sg(D9);
oldmon 0:8637a4213254 8
oldmon 0:8637a4213254 9 // Constructor ----------------------------------------------------------------
oldmon 0:8637a4213254 10 DigitalOut myled(LED1);
oldmon 0:8637a4213254 11 Serial pc(USBTX, USBRX);
oldmon 0:8637a4213254 12 VL53L0X sensor(I2C_SDA,I2C_SCL,NC); // XSHUT = NC
oldmon 0:8637a4213254 13 Timer t;
oldmon 0:8637a4213254 14
oldmon 0:8637a4213254 15 // ROM / Constant data --------------------------------------------------------
oldmon 0:8637a4213254 16 char *const msg0="VL53L0X is running correctly!!\r\n";
oldmon 0:8637a4213254 17 char *const msg1="VL53L0X -> something is wrong!!\r\n";
oldmon 0:8637a4213254 18 char *const msg2="#,";
oldmon 0:8637a4213254 19 char *const msg3="d[mm]=,";
oldmon 0:8637a4213254 20 char *const msg4="d[mm]=,error,";
oldmon 0:8637a4213254 21 char *const msg5="VL53[mS]=, ";
oldmon 0:8637a4213254 22 char *const msg6="all[mS]=, ";
oldmon 0:8637a4213254 23
oldmon 0:8637a4213254 24 //L298n connections
oldmon 0:8637a4213254 25 DigitalOut pinI1(D5);
oldmon 0:8637a4213254 26 DigitalOut pinI2(D6);
oldmon 0:8637a4213254 27 DigitalOut pinI3(D10);
oldmon 0:8637a4213254 28 DigitalOut pinI4(D11);
oldmon 0:8637a4213254 29
oldmon 0:8637a4213254 30 int main(){
oldmon 0:8637a4213254 31 int status=VL53L0X_ERROR_NONE;
oldmon 0:8637a4213254 32 uint32_t data;
oldmon 0:8637a4213254 33 uint32_t count=0;
oldmon 0:8637a4213254 34 uint32_t tm_sensor;
oldmon 0:8637a4213254 35 uint32_t tm_all_work;
oldmon 0:8637a4213254 36
oldmon 0:8637a4213254 37 #if USE_XSHUT
oldmon 0:8637a4213254 38 status = sensor.init_sensor(0x33); // set new I2C address
oldmon 0:8637a4213254 39 #else
oldmon 0:8637a4213254 40 // no control XSHUT then set default address (no other way)
oldmon 0:8637a4213254 41 status = sensor.init_sensor(VL53L0X_DEFAULT_ADDRESS);
oldmon 0:8637a4213254 42 #endif
oldmon 0:8637a4213254 43
oldmon 0:8637a4213254 44 if (status == VL53L0X_ERROR_NONE)
oldmon 0:8637a4213254 45 pc.printf(msg0);
oldmon 0:8637a4213254 46 else{
oldmon 0:8637a4213254 47 pc.printf(msg1);
oldmon 0:8637a4213254 48 pc.printf("%d\n",status);
oldmon 0:8637a4213254 49 }
oldmon 0:8637a4213254 50 //status = sensor.set_mode(range_long_distance_33ms_200cm);
oldmon 0:8637a4213254 51 status = sensor.set_mode(range_hi_accurate_200ms_120cm);
oldmon 0:8637a4213254 52 //status = sensor.set_mode(range_hi_speed_20ms_120cm);
oldmon 0:8637a4213254 53 if (status == VL53L0X_ERROR_NONE)
oldmon 0:8637a4213254 54 pc.printf(msg0);
oldmon 0:8637a4213254 55 else{
oldmon 0:8637a4213254 56 pc.printf(msg1);
oldmon 0:8637a4213254 57 pc.printf("%d\n",status);
oldmon 0:8637a4213254 58 }
oldmon 0:8637a4213254 59 while(true){
oldmon 0:8637a4213254 60 t.reset();
oldmon 0:8637a4213254 61 t.start();
oldmon 0:8637a4213254 62 //myled = !myled;
oldmon 0:8637a4213254 63 status = sensor.get_distance(&data);
oldmon 0:8637a4213254 64 tm_sensor = t.read_ms();
oldmon 0:8637a4213254 65 if (status == VL53L0X_ERROR_NONE)
oldmon 0:8637a4213254 66 pc.printf("%s%5d,%s%5d,", msg2, count++, msg3, data);
oldmon 0:8637a4213254 67 else
oldmon 0:8637a4213254 68 pc.printf("%s%5d,%s", msg2, count++, msg4);
oldmon 0:8637a4213254 69 pc.printf("%s%d,%s%d\r\n", msg5, tm_sensor, msg6, tm_all_work);
oldmon 0:8637a4213254 70 tm_all_work = t.read_ms();
oldmon 0:8637a4213254 71 if (tm_all_work < 99)
oldmon 0:8637a4213254 72 wait_ms(100 - tm_all_work);
oldmon 0:8637a4213254 73
oldmon 0:8637a4213254 74 /*sg.SetAngle(80);
oldmon 0:8637a4213254 75 wait_ms(1000);
oldmon 0:8637a4213254 76 sg.SetAngle(-80);
oldmon 0:8637a4213254 77 wait_ms(1000);
oldmon 0:8637a4213254 78 forward();
oldmon 0:8637a4213254 79 wait_ms(3000);
oldmon 0:8637a4213254 80 left();
oldmon 0:8637a4213254 81 wait_ms(1000);
oldmon 0:8637a4213254 82 right();
oldmon 0:8637a4213254 83 wait_ms(1000);
oldmon 0:8637a4213254 84 stop();*/
oldmon 0:8637a4213254 85 }
oldmon 0:8637a4213254 86 }
oldmon 0:8637a4213254 87
oldmon 0:8637a4213254 88 void reverse(){
oldmon 0:8637a4213254 89 pinI1=1;
oldmon 0:8637a4213254 90 pinI2=0;
oldmon 0:8637a4213254 91 pinI3=1;
oldmon 0:8637a4213254 92 pinI4=0;
oldmon 0:8637a4213254 93 }
oldmon 0:8637a4213254 94
oldmon 0:8637a4213254 95 void forward(){
oldmon 0:8637a4213254 96 pinI1=0;
oldmon 0:8637a4213254 97 pinI2=1;
oldmon 0:8637a4213254 98 pinI3=0;
oldmon 0:8637a4213254 99 pinI4=1;
oldmon 0:8637a4213254 100 }
oldmon 0:8637a4213254 101 void left(){
oldmon 0:8637a4213254 102 pinI1=0;
oldmon 0:8637a4213254 103 pinI2=1;
oldmon 0:8637a4213254 104 pinI3=0;
oldmon 0:8637a4213254 105 pinI4=0;
oldmon 0:8637a4213254 106 }
oldmon 0:8637a4213254 107 void right(){
oldmon 0:8637a4213254 108 pinI1=0;
oldmon 0:8637a4213254 109 pinI2=0;
oldmon 0:8637a4213254 110 pinI3=0;
oldmon 0:8637a4213254 111 pinI4=1;
oldmon 0:8637a4213254 112 }
oldmon 0:8637a4213254 113 void stop(){
oldmon 0:8637a4213254 114 pinI1=0;
oldmon 0:8637a4213254 115 pinI2=0;
oldmon 0:8637a4213254 116 pinI3=0;
oldmon 0:8637a4213254 117 pinI4=0;
oldmon 0:8637a4213254 118 }