David Buck
/
Car_2
Diff: main.cpp
- Revision:
- 0:e9d343cd983c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Feb 05 12:26:36 2010 +0000 @@ -0,0 +1,155 @@ +#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); + +LocalFileSystem local("local"); +InterruptIn w(p16); +Timer timer; + +void backward (float); +void forward (float); +void right (void); +void left (void); +void reset (void); +void stop (void); + + +//servo vars +int i = 0; +int j = 0; + +//timer/sensor vars +int count; +int begin; +int end; +int single_spin; +//const int wheel = 257; //in mm +//int Tdist = 0; + +//intrupt program exicuted +void trigger() { + + end = timer.read_ms(); //read timer + count = count + 1; //add to total turn count + single_spin = end - begin; //calculate total time for 1 spin + + FILE *fp = fopen("/local/out.csv", "a"); //open file in append + fprintf(fp, "%d, %d\n", count, single_spin);// add data to file + fclose(fp); //colse file + + begin = timer.read_ms(); // reset timer + + +} + + +int main() { + + timer.start(); //start timer + w.rise(&trigger); //set trigger + + 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