Revision 3:857732eebab2, committed 2018-06-19

Comitter:

oldmon

Date:

Tue Jun 19 11:28:46 2018 +0000

Parent:

2:64a1dd5f6dc7

Commit message:

working with fake data

Changed in this revision

--- a/main.cpp	Mon Jun 18 13:09:00 2018 +0000
+++ b/main.cpp	Tue Jun 19 11:28:46 2018 +0000
@@ -3,10 +3,9 @@
 #include "VL53L0X.h"
 #define USE_XSHUT   0
 
+//  Constructor ----------------------------------------------------------------
 DigitalIn Btn1(PC_13);//  USER_BUTTON
 SG90 sg(D9);
-
-//  Constructor ----------------------------------------------------------------
 DigitalOut  myled(LED1);
 Serial      pc(USBTX, USBRX);
 VL53L0X     sensor(I2C_SDA,I2C_SCL,NC);        // XSHUT = NC
@@ -27,12 +26,17 @@
 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
@@ -41,7 +45,7 @@
     status = sensor.init_sensor(VL53L0X_DEFAULT_ADDRESS);
 	#endif
 
-    if (status == VL53L0X_ERROR_NONE)
+    if(status == VL53L0X_ERROR_NONE)
         pc.printf(msg0);
     else{
         pc.printf(msg1);
@@ -57,7 +61,7 @@
         pc.printf("%d\n",status);
         }
     while(true){
-        t.reset();
+        /*t.reset();
         t.start();
         //myled = !myled;
         status = sensor.get_distance(&data);
@@ -69,8 +73,17 @@
         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);
-
+            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);
@@ -82,12 +95,7 @@
 	right();
 	wait_ms(1000);
 	stop();*/
-
-	/*forward();						//輪胎馬達前進
-	if(LiDAR_Distance <= 30)		//LiDAR_Distance 由 LiDAR sensor 獲得距離
-	{
-		TurnLeft_or_TurnRight();
-	}*/
+	}
 }
 
 void reverse(){
@@ -96,6 +104,7 @@
     pinI3=1;
     pinI4=0;
 }
+
 void forward(){
     pinI1=0;
     pinI2=1;
@@ -107,28 +116,45 @@
     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(){
-			sg.SetAngle(-80);		//伺服馬達轉向左
-			LeftDistance = LiDAR_Distance;		//雷達測距並存到 LeftDistance
-			
-			sg.SetAngle(80);		//伺服馬達轉向右
-			RightDistance = LiDAR_Distance;		//雷達測距並存到 RightDistance
-			
-			if(LeftDistance > RightDistance)		//比較左右邊的距離
-				left();
-			else 
-				right();
-}*/
\ No newline at end of file
+	}
+	
+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);		//伺服馬達回正
+}
\ No newline at end of file