![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
3rd instalment of car automation
main.cpp
- Committer:
- Kaikestu
- Date:
- 2010-02-12
- Revision:
- 0:c6cbcb90e098
File content as of revision 0:c6cbcb90e098:
#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 writeBegin; int writeEnd; int single_spin; int ws; //const int wheel = 257; //in mm //int Tdist = 0; //intrupt program exicuted void trigger() { begin = timer.read_ms(); single_spin = begin - end; end = timer.read_ms(); writeBegin = timer.read_ms(); FILE *fp = fopen("/local/out.csv", "a"); count = count + 1; //myled2 = !myled2; fprintf(fp, "%d, %d,%d\n", count, single_spin, ws); fclose(fp); writeEnd = timer.read_ms(); ws = writeEnd - writeBegin; end = end - ws; } 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); }