Lu-Hsuan Chen
/
motorcar_pid
motor car with PID running feature
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
Ping.lib | Show annotated file Show diff for this revision Revisions of this file |
motorcar/motorcar.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 982dcc2390a2 -r 3caf5b6ed35a Ping.lib --- /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
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;