Not working yet
Dependencies: SG90 VL53L0X_simple mbed
main.cpp
- Committer:
- oldmon
- Date:
- 2018-06-19
- Revision:
- 3:857732eebab2
- Parent:
- 2:64a1dd5f6dc7
File content as of revision 3:857732eebab2:
#include "mbed.h" #include "SG90.h" #include "VL53L0X.h" #define USE_XSHUT 0 // Constructor ---------------------------------------------------------------- DigitalIn Btn1(PC_13);// USER_BUTTON SG90 sg(D9); DigitalOut myled(LED1); Serial pc(USBTX, USBRX); VL53L0X sensor(I2C_SDA,I2C_SCL,NC); // XSHUT = NC Timer t; // ROM / Constant data -------------------------------------------------------- char *const msg0="VL53L0X is running correctly!!\r\n"; char *const msg1="VL53L0X -> something is wrong!!\r\n"; char *const msg2="#,"; char *const msg3="d[mm]=,"; char *const msg4="d[mm]=,error,"; char *const msg5="VL53[mS]=, "; char *const msg6="all[mS]=, "; //L298n connections DigitalOut pinI1(D5); DigitalOut pinI2(D6); DigitalOut pinI3(D10); DigitalOut pinI4(D11); void forward(void); void stop(void); void TurnLeft_or_TurnRight(void); int main(){ int status=VL53L0X_ERROR_NONE; uint32_t data; uint32_t count=0; uint32_t tm_sensor; uint32_t tm_all_work; int LiDAR_Distance=40; #if USE_XSHUT status = sensor.init_sensor(0x33); // set new I2C address #else // no control XSHUT then set default address (no other way) status = sensor.init_sensor(VL53L0X_DEFAULT_ADDRESS); #endif if(status == VL53L0X_ERROR_NONE) pc.printf(msg0); else{ pc.printf(msg1); pc.printf("%d\n",status); } //status = sensor.set_mode(range_long_distance_33ms_200cm); status = sensor.set_mode(range_hi_accurate_200ms_120cm); //status = sensor.set_mode(range_hi_speed_20ms_120cm); if (status == VL53L0X_ERROR_NONE) pc.printf(msg0); else{ pc.printf(msg1); pc.printf("%d\n",status); } while(true){ /*t.reset(); t.start(); //myled = !myled; status = sensor.get_distance(&data); tm_sensor = t.read_ms(); if (status == VL53L0X_ERROR_NONE) pc.printf("%s%5d,%s%5d,", msg2, count++, msg3, data); else pc.printf("%s%5d,%s", msg2, count++, msg4); pc.printf("%s%d,%s%d\r\n", msg5, tm_sensor, msg6, tm_all_work); tm_all_work = t.read_ms(); if (tm_all_work < 99) wait_ms(100 - tm_all_work);*/ status = sensor.get_distance(&data); //if (status == VL53L0X_ERROR_NONE) //LiDAR_Distance = data; if(LiDAR_Distance <= 30){ //LiDAR_Distance 由 LiDAR sensor 獲得距離 stop(); TurnLeft_or_TurnRight(); }else forward(); //輪胎馬達前進 wait(5); LiDAR_Distance -= 5; /*sg.SetAngle(80); wait_ms(1000); sg.SetAngle(-80); wait_ms(1000); forward(); wait_ms(3000); left(); wait_ms(1000); right(); wait_ms(1000); stop();*/ } } void reverse(){ pinI1=1; pinI2=0; pinI3=1; pinI4=0; } void forward(){ pinI1=0; pinI2=1; pinI3=0; pinI4=1; } void left(){ pinI1=0; pinI2=1; pinI3=0; pinI4=0; } void right(){ pinI1=0; pinI2=0; pinI3=0; pinI4=1; } void stop(){ pinI1=0; pinI2=0; pinI3=0; pinI4=0; } void TurnLeft_or_TurnRight(){ int status; uint32_t data; uint32_t LeftDistance; uint32_t RightDistance; sg.SetAngle(-80); //伺服馬達轉向左 status = sensor.get_distance(&data); if (status == VL53L0X_ERROR_NONE) LeftDistance = data; //雷達測距並存到 LeftDistance sg.SetAngle(80); //伺服馬達轉向左 status = sensor.get_distance(&data); if (status == VL53L0X_ERROR_NONE) RightDistance = data; //雷達測距並存到 RightDistance //假資料區 LeftDistance=55; RightDistance=65; if(LeftDistance > RightDistance){ //比較左右邊的距離 left(); wait(0.5); }else{ right(); wait(0.5); } sg.SetAngle(0); //伺服馬達回正 }