David Buck
/
Car_1
Diff: main.cpp
- Revision:
- 0:eff3ddf69354
diff -r 000000000000 -r eff3ddf69354 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Feb 05 12:25:12 2010 +0000 @@ -0,0 +1,124 @@ +#include "mbed.h" +#include "Servo.h" +#include "SRF05.h" + +Servo motor (p21); +Servo turn (p22); + +SRF05 front (p24,p23); +SRF05 back (p26,p25); + +DigitalOut b (p20); +DigitalOut led1 (LED1); +DigitalOut led2 (LED2); + +void backward (float); +void forward (float); +void right (void); +void left (void); +void reset (void); +void stop (void); + + +int i = 0; +int j = 0; + +float sample_delay = 0.01; + +int main() { + + b = 0; + reset(); + + //main loop + while (1) { + + //init + i = front.read(); + j = back.read(); + + + //can go forward? + if (i > 100) { + while (i > 100) { + + forward(20); + i = front.read(); + j = back.read(); + } + + stop(); + } + + //can it turn to avoid? + if (i > 10 && i < 100) { + led1 = 1; + while (i > 10 && i < 100) { + + forward(20); + turn.write(0); + + i = front.read(); + j = back.read(); + } + led1 = 0; + stop(); + } + + //is there room to ververse and turn? + if (i < 20 && j > 10) { + led2 = 1; + while (i < 20 && j > 10) { + + backward(20); + turn.write(1); + + i = front.read(); + j = back.read(); + } + led2 = 0; + stop(); + } + } +} + +void reset (void) { + turn.write(0.5); + motor.write(0.48); +} + +// 0.5 = 0 speed & 0 = full speed +void forward (float speed) { + float y = speed / 100; + float x = y * 0.5; + float z = 0.5 - x; + + motor.write(z); +} + +// 0.5 = 0 speed & 1 = full speed + +void backward (float speed) { + float y = speed / 100; + float x = y * 0.5; + float z = 0.5 + x; + + motor.write(z); +} + +// 0 = right 0.5 = straight 1 = left +void left (void) { + turn.write(1); +} +void right (void) { + turn.write(0); +} + +void stop (void) { + b = 1; + wait(0.3); + reset(); + wait(0.1); + b = 0; + wait(1); +} \ No newline at end of file