Lu-Hsuan Chen
/
motorcar_pid
motor car with PID running feature
Diff: motorcar/motorcar.cpp
- Revision:
- 5:3caf5b6ed35a
- Parent:
- 4:982dcc2390a2
diff -r 982dcc2390a2 -r 3caf5b6ed35a motorcar/motorcar.cpp --- 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;