Jenhao Ou / Mbed 2 deprecated lidarproj

Not working yet

Dependencies:   SG90 VL53L0X_simple mbed

main.cpp@3:857732eebab2, 2018-06-19 (annotated)

Committer:
oldmon
Date:
Tue Jun 19 11:28:46 2018 +0000
Revision:
3:857732eebab2
Parent:
2:64a1dd5f6dc7 
working with fake data

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 3:857732eebab2 6 // Constructor ----------------------------------------------------------------
oldmon 0:8637a4213254 7 DigitalIn Btn1(PC_13);// USER_BUTTON
oldmon 0:8637a4213254 8 SG90 sg(D9);
oldmon 0:8637a4213254 9 DigitalOut myled(LED1);
oldmon 0:8637a4213254 10 Serial pc(USBTX, USBRX);
oldmon 0:8637a4213254 11 VL53L0X sensor(I2C_SDA,I2C_SCL,NC); // XSHUT = NC
oldmon 0:8637a4213254 12 Timer t;
oldmon 0:8637a4213254 13
oldmon 0:8637a4213254 14 // ROM / Constant data --------------------------------------------------------
oldmon 0:8637a4213254 15 char *const msg0="VL53L0X is running correctly!!\r\n";
oldmon 0:8637a4213254 16 char *const msg1="VL53L0X -> something is wrong!!\r\n";
oldmon 0:8637a4213254 17 char *const msg2="#,";
oldmon 0:8637a4213254 18 char *const msg3="d[mm]=,";
oldmon 0:8637a4213254 19 char *const msg4="d[mm]=,error,";
oldmon 0:8637a4213254 20 char *const msg5="VL53[mS]=, ";
oldmon 0:8637a4213254 21 char *const msg6="all[mS]=, ";
oldmon 0:8637a4213254 22
oldmon 0:8637a4213254 23 //L298n connections
oldmon 0:8637a4213254 24 DigitalOut pinI1(D5);
oldmon 0:8637a4213254 25 DigitalOut pinI2(D6);
oldmon 0:8637a4213254 26 DigitalOut pinI3(D10);
oldmon 0:8637a4213254 27 DigitalOut pinI4(D11);
oldmon 0:8637a4213254 28
oldmon 3:857732eebab2 29 void forward(void);
oldmon 3:857732eebab2 30 void stop(void);
oldmon 3:857732eebab2 31 void TurnLeft_or_TurnRight(void);
oldmon 3:857732eebab2 32
oldmon 0:8637a4213254 33 int main(){
oldmon 0:8637a4213254 34 int status=VL53L0X_ERROR_NONE;
oldmon 0:8637a4213254 35 uint32_t data;
oldmon 0:8637a4213254 36 uint32_t count=0;
oldmon 0:8637a4213254 37 uint32_t tm_sensor;
oldmon 0:8637a4213254 38 uint32_t tm_all_work;
oldmon 3:857732eebab2 39 int LiDAR_Distance=40;
oldmon 0:8637a4213254 40
oldmon 0:8637a4213254 41 #if USE_XSHUT
oldmon 0:8637a4213254 42 status = sensor.init_sensor(0x33); // set new I2C address
oldmon 0:8637a4213254 43 #else
oldmon 0:8637a4213254 44 // no control XSHUT then set default address (no other way)
oldmon 0:8637a4213254 45 status = sensor.init_sensor(VL53L0X_DEFAULT_ADDRESS);
oldmon 0:8637a4213254 46 #endif
oldmon 0:8637a4213254 47
oldmon 3:857732eebab2 48 if(status == VL53L0X_ERROR_NONE)
oldmon 0:8637a4213254 49 pc.printf(msg0);
oldmon 0:8637a4213254 50 else{
oldmon 0:8637a4213254 51 pc.printf(msg1);
oldmon 0:8637a4213254 52 pc.printf("%d\n",status);
oldmon 0:8637a4213254 53 }
oldmon 0:8637a4213254 54 //status = sensor.set_mode(range_long_distance_33ms_200cm);
oldmon 0:8637a4213254 55 status = sensor.set_mode(range_hi_accurate_200ms_120cm);
oldmon 0:8637a4213254 56 //status = sensor.set_mode(range_hi_speed_20ms_120cm);
oldmon 0:8637a4213254 57 if (status == VL53L0X_ERROR_NONE)
oldmon 0:8637a4213254 58 pc.printf(msg0);
oldmon 0:8637a4213254 59 else{
oldmon 0:8637a4213254 60 pc.printf(msg1);
oldmon 0:8637a4213254 61 pc.printf("%d\n",status);
oldmon 0:8637a4213254 62 }
oldmon 0:8637a4213254 63 while(true){
oldmon 3:857732eebab2 64 /*t.reset();
oldmon 0:8637a4213254 65 t.start();
oldmon 0:8637a4213254 66 //myled = !myled;
oldmon 0:8637a4213254 67 status = sensor.get_distance(&data);
oldmon 0:8637a4213254 68 tm_sensor = t.read_ms();
oldmon 0:8637a4213254 69 if (status == VL53L0X_ERROR_NONE)
oldmon 0:8637a4213254 70 pc.printf("%s%5d,%s%5d,", msg2, count++, msg3, data);
oldmon 0:8637a4213254 71 else
oldmon 0:8637a4213254 72 pc.printf("%s%5d,%s", msg2, count++, msg4);
oldmon 0:8637a4213254 73 pc.printf("%s%d,%s%d\r\n", msg5, tm_sensor, msg6, tm_all_work);
oldmon 0:8637a4213254 74 tm_all_work = t.read_ms();
oldmon 0:8637a4213254 75 if (tm_all_work < 99)
oldmon 3:857732eebab2 76 wait_ms(100 - tm_all_work);*/
oldmon 3:857732eebab2 77 status = sensor.get_distance(&data);
oldmon 3:857732eebab2 78 //if (status == VL53L0X_ERROR_NONE)
oldmon 3:857732eebab2 79 //LiDAR_Distance = data;
oldmon 3:857732eebab2 80 if(LiDAR_Distance <= 30){ //LiDAR_Distance 由 LiDAR sensor 獲得距離
oldmon 3:857732eebab2 81 stop();
oldmon 3:857732eebab2 82 TurnLeft_or_TurnRight();
oldmon 3:857732eebab2 83 }else
oldmon 3:857732eebab2 84 forward(); //輪胎馬達前進
oldmon 3:857732eebab2 85 wait(5);
oldmon 3:857732eebab2 86 LiDAR_Distance -= 5;
oldmon 0:8637a4213254 87 /*sg.SetAngle(80);
oldmon 0:8637a4213254 88 wait_ms(1000);
oldmon 0:8637a4213254 89 sg.SetAngle(-80);
oldmon 0:8637a4213254 90 wait_ms(1000);
oldmon 0:8637a4213254 91 forward();
oldmon 0:8637a4213254 92 wait_ms(3000);
oldmon 0:8637a4213254 93 left();
oldmon 0:8637a4213254 94 wait_ms(1000);
oldmon 0:8637a4213254 95 right();
oldmon 0:8637a4213254 96 wait_ms(1000);
oldmon 0:8637a4213254 97 stop();*/
oldmon 3:857732eebab2 98 }
oldmon 0:8637a4213254 99 }
oldmon 0:8637a4213254 100
oldmon 0:8637a4213254 101 void reverse(){
oldmon 0:8637a4213254 102 pinI1=1;
oldmon 0:8637a4213254 103 pinI2=0;
oldmon 0:8637a4213254 104 pinI3=1;
oldmon 0:8637a4213254 105 pinI4=0;
oldmon 0:8637a4213254 106 }
oldmon 3:857732eebab2 107
oldmon 0:8637a4213254 108 void forward(){
oldmon 0:8637a4213254 109 pinI1=0;
oldmon 0:8637a4213254 110 pinI2=1;
oldmon 0:8637a4213254 111 pinI3=0;
oldmon 0:8637a4213254 112 pinI4=1;
oldmon 0:8637a4213254 113 }
oldmon 0:8637a4213254 114 void left(){
oldmon 0:8637a4213254 115 pinI1=0;
oldmon 0:8637a4213254 116 pinI2=1;
oldmon 0:8637a4213254 117 pinI3=0;
oldmon 0:8637a4213254 118 pinI4=0;
oldmon 3:857732eebab2 119 }
oldmon 0:8637a4213254 120 void right(){
oldmon 0:8637a4213254 121 pinI1=0;
oldmon 0:8637a4213254 122 pinI2=0;
oldmon 0:8637a4213254 123 pinI3=0;
oldmon 0:8637a4213254 124 pinI4=1;
oldmon 3:857732eebab2 125 }
oldmon 0:8637a4213254 126 void stop(){
oldmon 0:8637a4213254 127 pinI1=0;
oldmon 0:8637a4213254 128 pinI2=0;
oldmon 0:8637a4213254 129 pinI3=0;
oldmon 0:8637a4213254 130 pinI4=0;
oldmon 3:857732eebab2 131 }
oldmon 3:857732eebab2 132
oldmon 3:857732eebab2 133 void TurnLeft_or_TurnRight(){
oldmon 3:857732eebab2 134 int status;
oldmon 3:857732eebab2 135 uint32_t data;
oldmon 3:857732eebab2 136 uint32_t LeftDistance;
oldmon 3:857732eebab2 137 uint32_t RightDistance;
oldmon 3:857732eebab2 138 sg.SetAngle(-80); //伺服馬達轉向左
oldmon 3:857732eebab2 139 status = sensor.get_distance(&data);
oldmon 3:857732eebab2 140 if (status == VL53L0X_ERROR_NONE)
oldmon 3:857732eebab2 141 LeftDistance = data; //雷達測距並存到 LeftDistance
oldmon 3:857732eebab2 142
oldmon 3:857732eebab2 143 sg.SetAngle(80); //伺服馬達轉向左
oldmon 3:857732eebab2 144 status = sensor.get_distance(&data);
oldmon 3:857732eebab2 145 if (status == VL53L0X_ERROR_NONE)
oldmon 3:857732eebab2 146 RightDistance = data; //雷達測距並存到 RightDistance
oldmon 3:857732eebab2 147
oldmon 3:857732eebab2 148 //假資料區
oldmon 3:857732eebab2 149 LeftDistance=55;
oldmon 3:857732eebab2 150 RightDistance=65;
oldmon 3:857732eebab2 151
oldmon 3:857732eebab2 152 if(LeftDistance > RightDistance){ //比較左右邊的距離
oldmon 3:857732eebab2 153 left();
oldmon 3:857732eebab2 154 wait(0.5);
oldmon 3:857732eebab2 155 }else{
oldmon 3:857732eebab2 156 right();
oldmon 3:857732eebab2 157 wait(0.5);
oldmon 3:857732eebab2 158 }
oldmon 3:857732eebab2 159 sg.SetAngle(0); //伺服馬達回正
oldmon 3:857732eebab2 160 }