main.cpp

00001 #include "mbed.h"
00002 #include "Servo.h"
00003 #include "SRF05.h"
00004 
00005 Servo motor (p21);
00006 Servo turn (p22);
00007 
00008 SRF05 front (p24,p23);
00009 SRF05 back (p26,p25);
00010 
00011 DigitalOut b (p20);
00012 DigitalOut led1 (LED1);
00013 DigitalOut led2 (LED2);
00014 
00015 LocalFileSystem local("local");
00016 InterruptIn w(p16);
00017 Timer timer;
00018 
00019 void backward (float);
00020 void forward (float);
00021 void right (void);
00022 void left (void);
00023 void reset (void);
00024 void stop (void);
00025 
00026 
00027 //servo vars
00028 int i = 0;
00029 int j = 0;
00030 
00031 //timer/sensor vars
00032 int count;
00033 int begin;
00034 int end;
00035 int writeBegin;
00036 int writeEnd;
00037 int single_spin;
00038 int ws;
00039 //const int wheel = 257; //in mm
00040 //int Tdist = 0;
00041 
00042 //intrupt program exicuted
00043 void trigger() {
00044 
00045     begin = timer.read_ms();
00046     single_spin = begin - end;
00047     end = timer.read_ms();
00048     
00049     
00050     writeBegin = timer.read_ms();
00051     FILE *fp = fopen("/local/out.csv", "a");
00052     count = count + 1;
00053     //myled2 = !myled2;
00054     fprintf(fp, "%d, %d,%d\n", count, single_spin, ws);
00055     fclose(fp);
00056     
00057     writeEnd = timer.read_ms();       
00058     ws = writeEnd - writeBegin;
00059     end = end - ws;
00060 
00061 }
00062 
00063 
00064 int main() {
00065 
00066     timer.start(); //start timer
00067     w.rise(&trigger); //set trigger
00068 
00069     b = 0;
00070     reset();
00071 
00072     //main loop
00073     while (1) {
00074 
00075         //init
00076         i = front.read();
00077         j = back.read();
00078 
00079 
00080         //can go forward?
00081         if (i > 100) {
00082             while (i > 100) {
00083 
00084                 forward(20);
00085                 i = front.read();
00086                 j = back.read();
00087             }
00088 
00089             stop();
00090         }
00091 
00092         //can it turn to avoid?
00093         if (i > 10 && i < 100) {
00094             led1 = 1;
00095             while (i > 10 && i < 100) {
00096 
00097                 forward(20);
00098                 turn.write(0);
00099 
00100                 i = front.read();
00101                 j = back.read();
00102             }
00103             led1 = 0;
00104             stop();
00105         }
00106 
00107         //is there room to ververse and turn?
00108         if (i < 20 && j > 10) {
00109             led2 = 1;
00110             while (i < 20 && j > 10) {
00111 
00112                 backward(20);
00113                 turn.write(1);
00114 
00115                 i = front.read();
00116                 j = back.read();
00117             }
00118             led2 = 0;
00119             stop();
00120         }
00121     }
00122 }
00123 
00124 void reset (void) {
00125     turn.write(0.5);
00126     motor.write(0.48);
00127 }
00128 
00129 // 0.5 = 0 speed & 0 = full speed
00130 void forward (float speed) {
00131     float y = speed / 100;
00132     float x = y * 0.5;
00133     float z = 0.5 - x;
00134 
00135     motor.write(z);
00136 }
00137 
00138 // 0.5 = 0 speed & 1 = full speed
00139 
00140 void backward (float speed) {
00141     float y = speed / 100;
00142     float x = y * 0.5;
00143     float z = 0.5 + x;
00144 
00145     motor.write(z);
00146 }
00147 
00148 // 0 = right 0.5 = straight 1 = left
00149 void left (void) {
00150     turn.write(1);
00151 }
00152 void right (void) {
00153     turn.write(0);
00154 }
00155 
00156 void stop (void) {
00157     b = 1;
00158     wait(0.3);
00159     reset();
00160     wait(0.1);
00161     b = 0;
00162     wait(1);
00163 }