David Buck
/
Car_1
main.cpp
- Committer:
- Kaikestu
- Date:
- 2010-02-05
- Revision:
- 0:eff3ddf69354
File content as of revision 0:eff3ddf69354:
#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); }