Diff: motorcar/motorcar.cpp

Revision:

5:3caf5b6ed35a

Parent:

4:982dcc2390a2

--- 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;