Revision 5:3caf5b6ed35a, committed 2018-08-13

Comitter:

cudaChen

Date:

Mon Aug 13 08:49:25 2018 +0000

Parent:

4:982dcc2390a2

Commit message:

[add] add lines for obstacle aviodance

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Ping.lib	Mon Aug 13 08:49:25 2018 +0000
@@ -0,0 +1,1 @@
+http://os.mbed.com/users/rosienej/code/Ping/#6996f66161d7

--- a/motorcar/motorcar.cpp	Wed Aug 01 08:24:24 2018 +0000
+++ b/motorcar/motorcar.cpp	Mon Aug 13 08:49:25 2018 +0000
@@ -1,7 +1,7 @@
 #include "mbed.h"
 
 #include "motorcar.h"
-//#include "Ping.h"
+#include "Ping.h"
 
 // output used for controlling motors
 AnalogOut M1_enable(PA_4); // right motor enable signal
@@ -19,6 +19,12 @@
 // used for storing returned IR sensors' value
 int left, middle, right;
 
+// used for ultra sonic sensor
+AnalogIn Ping ultraSonic(D12);
+int distance;
+bool hasObstacle = false;
+int range = 20;
+
 // PID factors
 int interror = 0;
 int olderror = 0;
@@ -34,6 +40,11 @@
 //Serial pc(SERIAL_TX, SERIAL_RX);
 
 void readSensorValue() {
+    ultraSonic.Send();
+    wait_ms(30);
+    distance = ultraSonic.Read_cm();
+    hasObstacle = distance < range ? true : false;
+    
     left = leftIR.read_u16();
     middle = middleIR.read_u16();
     right = rightIR.read_u16();
@@ -46,6 +57,25 @@
 }
 
 void runPID() {
+    // if there is an obstacle
+    if(hasObstacle) {
+        speed(0, 0);
+        wait_ms(200);
+        speed(0, 200);
+        wait_ms(200);
+        speed(0, 0);
+        wait_ms(200);
+        speed(200, 200);
+        wait_ms(800);
+        speed(0, 0);
+        wait_ms(200);
+        speed(200, 0);
+        wait_ms(200);
+        speed(0, 0);
+        wait_ms(200);
+        return;
+    }
+    
     int error = values;
     interror += error;
     int lasterror = error - olderror;