![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Not working yet
Dependencies: SG90 VL53L0X_simple mbed
main.cpp@2:64a1dd5f6dc7, 2018-06-18 (annotated)
- Committer:
- oldmon
- Date:
- Mon Jun 18 13:09:00 2018 +0000
- Revision:
- 2:64a1dd5f6dc7
- Parent:
- 0:8637a4213254
- Child:
- 3:857732eebab2
added GRAFCETED
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 | 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 | 2:64a1dd5f6dc7 | 85 | |
oldmon | 2:64a1dd5f6dc7 | 86 | /*forward(); //輪胎馬達前進 |
oldmon | 2:64a1dd5f6dc7 | 87 | if(LiDAR_Distance <= 30) //LiDAR_Distance 由 LiDAR sensor 獲得距離 |
oldmon | 2:64a1dd5f6dc7 | 88 | { |
oldmon | 2:64a1dd5f6dc7 | 89 | TurnLeft_or_TurnRight(); |
oldmon | 2:64a1dd5f6dc7 | 90 | }*/ |
oldmon | 0:8637a4213254 | 91 | } |
oldmon | 0:8637a4213254 | 92 | |
oldmon | 0:8637a4213254 | 93 | void reverse(){ |
oldmon | 0:8637a4213254 | 94 | pinI1=1; |
oldmon | 0:8637a4213254 | 95 | pinI2=0; |
oldmon | 0:8637a4213254 | 96 | pinI3=1; |
oldmon | 0:8637a4213254 | 97 | pinI4=0; |
oldmon | 0:8637a4213254 | 98 | } |
oldmon | 0:8637a4213254 | 99 | void forward(){ |
oldmon | 0:8637a4213254 | 100 | pinI1=0; |
oldmon | 0:8637a4213254 | 101 | pinI2=1; |
oldmon | 0:8637a4213254 | 102 | pinI3=0; |
oldmon | 0:8637a4213254 | 103 | pinI4=1; |
oldmon | 0:8637a4213254 | 104 | } |
oldmon | 0:8637a4213254 | 105 | void left(){ |
oldmon | 0:8637a4213254 | 106 | pinI1=0; |
oldmon | 0:8637a4213254 | 107 | pinI2=1; |
oldmon | 0:8637a4213254 | 108 | pinI3=0; |
oldmon | 0:8637a4213254 | 109 | pinI4=0; |
oldmon | 2:64a1dd5f6dc7 | 110 | } |
oldmon | 0:8637a4213254 | 111 | void right(){ |
oldmon | 0:8637a4213254 | 112 | pinI1=0; |
oldmon | 0:8637a4213254 | 113 | pinI2=0; |
oldmon | 0:8637a4213254 | 114 | pinI3=0; |
oldmon | 0:8637a4213254 | 115 | pinI4=1; |
oldmon | 2:64a1dd5f6dc7 | 116 | } |
oldmon | 0:8637a4213254 | 117 | void stop(){ |
oldmon | 0:8637a4213254 | 118 | pinI1=0; |
oldmon | 0:8637a4213254 | 119 | pinI2=0; |
oldmon | 0:8637a4213254 | 120 | pinI3=0; |
oldmon | 0:8637a4213254 | 121 | pinI4=0; |
oldmon | 2:64a1dd5f6dc7 | 122 | } |
oldmon | 2:64a1dd5f6dc7 | 123 | /*void TurnLeft_or_TurnRight(){ |
oldmon | 2:64a1dd5f6dc7 | 124 | sg.SetAngle(-80); //伺服馬達轉向左 |
oldmon | 2:64a1dd5f6dc7 | 125 | LeftDistance = LiDAR_Distance; //雷達測距並存到 LeftDistance |
oldmon | 2:64a1dd5f6dc7 | 126 | |
oldmon | 2:64a1dd5f6dc7 | 127 | sg.SetAngle(80); //伺服馬達轉向右 |
oldmon | 2:64a1dd5f6dc7 | 128 | RightDistance = LiDAR_Distance; //雷達測距並存到 RightDistance |
oldmon | 2:64a1dd5f6dc7 | 129 | |
oldmon | 2:64a1dd5f6dc7 | 130 | if(LeftDistance > RightDistance) //比較左右邊的距離 |
oldmon | 2:64a1dd5f6dc7 | 131 | left(); |
oldmon | 2:64a1dd5f6dc7 | 132 | else |
oldmon | 2:64a1dd5f6dc7 | 133 | right(); |
oldmon | 2:64a1dd5f6dc7 | 134 | }*/ |