![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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?
User | Revision | Line number | New 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 | } |