main.cpp

00001 #include "mbed.h"
00002 #include "SG90.h"
00003 #include "VL53L0X.h"
00004 #define USE_XSHUT   0
00005 
00006 //  Constructor ----------------------------------------------------------------
00007 DigitalIn Btn1(PC_13);//  USER_BUTTON
00008 SG90 sg(D9);
00009 DigitalOut  myled(LED1);
00010 Serial      pc(USBTX, USBRX);
00011 VL53L0X     sensor(I2C_SDA,I2C_SCL,NC);        // XSHUT = NC
00012 Timer       t;
00013 
00014 //  ROM / Constant data --------------------------------------------------------
00015 char *const msg0="VL53L0X is running correctly!!\r\n";
00016 char *const msg1="VL53L0X -> something is wrong!!\r\n";
00017 char *const msg2="#,";
00018 char *const msg3="d[mm]=,";
00019 char *const msg4="d[mm]=,error,";
00020 char *const msg5="VL53[mS]=, ";
00021 char *const msg6="all[mS]=, ";
00022 
00023 //L298n connections
00024 DigitalOut pinI1(D5);
00025 DigitalOut pinI2(D6);
00026 DigitalOut pinI3(D10);
00027 DigitalOut pinI4(D11);
00028 
00029 void forward(void);
00030 void stop(void);
00031 void TurnLeft_or_TurnRight(void);
00032 
00033 int main(){
00034     int status=VL53L0X_ERROR_NONE;
00035     uint32_t data;
00036     uint32_t count=0;
00037     uint32_t tm_sensor;
00038     uint32_t tm_all_work;
00039     int LiDAR_Distance=40;
00040     
00041     #if USE_XSHUT
00042     status = sensor.init_sensor(0x33);  // set new I2C address
00043     #else
00044     // no control XSHUT then set default address (no other way)
00045     status = sensor.init_sensor(VL53L0X_DEFAULT_ADDRESS);
00046     #endif
00047 
00048     if(status == VL53L0X_ERROR_NONE)
00049         pc.printf(msg0);
00050     else{
00051         pc.printf(msg1);
00052         pc.printf("%d\n",status);
00053         }
00054     //status = sensor.set_mode(range_long_distance_33ms_200cm);
00055     status = sensor.set_mode(range_hi_accurate_200ms_120cm);
00056     //status = sensor.set_mode(range_hi_speed_20ms_120cm);
00057     if (status == VL53L0X_ERROR_NONE)
00058         pc.printf(msg0);
00059     else{
00060         pc.printf(msg1);
00061         pc.printf("%d\n",status);
00062         }
00063     while(true){
00064         /*t.reset();
00065         t.start();
00066         //myled = !myled;
00067         status = sensor.get_distance(&data);
00068         tm_sensor = t.read_ms();
00069         if (status == VL53L0X_ERROR_NONE)
00070             pc.printf("%s%5d,%s%5d,", msg2, count++, msg3, data);
00071         else
00072             pc.printf("%s%5d,%s", msg2, count++, msg4);
00073         pc.printf("%s%d,%s%d\r\n", msg5, tm_sensor, msg6, tm_all_work);
00074         tm_all_work = t.read_ms();
00075         if (tm_all_work < 99)
00076             wait_ms(100 - tm_all_work);*/
00077         status = sensor.get_distance(&data);
00078         //if (status == VL53L0X_ERROR_NONE)
00079         //LiDAR_Distance = data;
00080         if(LiDAR_Distance <= 30){       //LiDAR_Distance 由 LiDAR sensor 獲得距離
00081             stop();
00082             TurnLeft_or_TurnRight();
00083         }else
00084             forward();      //輪胎馬達前進
00085         wait(5);
00086         LiDAR_Distance -= 5;
00087     /*sg.SetAngle(80);
00088     wait_ms(1000);
00089     sg.SetAngle(-80);
00090     wait_ms(1000);
00091     forward();
00092     wait_ms(3000);
00093     left();
00094     wait_ms(1000);
00095     right();
00096     wait_ms(1000);
00097     stop();*/
00098     }
00099 }
00100 
00101 void reverse(){
00102     pinI1=1;
00103     pinI2=0;
00104     pinI3=1;
00105     pinI4=0;
00106 }
00107 
00108 void forward(){
00109     pinI1=0;
00110     pinI2=1;
00111     pinI3=0;
00112     pinI4=1;
00113 }
00114 void left(){
00115     pinI1=0;
00116     pinI2=1;
00117     pinI3=0;
00118     pinI4=0;
00119     }
00120 void right(){
00121     pinI1=0;
00122     pinI2=0;
00123     pinI3=0;
00124     pinI4=1;
00125     }
00126 void stop(){
00127     pinI1=0;
00128     pinI2=0;
00129     pinI3=0;
00130     pinI4=0;
00131     }
00132     
00133 void TurnLeft_or_TurnRight(){
00134     int status;
00135     uint32_t data;
00136     uint32_t LeftDistance;
00137     uint32_t RightDistance;
00138     sg.SetAngle(-80);       //伺服馬達轉向左
00139     status = sensor.get_distance(&data);
00140     if (status == VL53L0X_ERROR_NONE)
00141     LeftDistance = data;        //雷達測距並存到 LeftDistance
00142     
00143     sg.SetAngle(80);        //伺服馬達轉向左
00144     status = sensor.get_distance(&data);
00145     if (status == VL53L0X_ERROR_NONE)
00146     RightDistance = data;       //雷達測距並存到 RightDistance
00147     
00148     //假資料區
00149     LeftDistance=55;
00150     RightDistance=65;
00151     
00152     if(LeftDistance > RightDistance){       //比較左右邊的距離
00153         left();
00154         wait(0.5);
00155     }else{
00156         right();
00157         wait(0.5);
00158     }
00159     sg.SetAngle(0);     //伺服馬達回正
00160 }