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 single_spin;
00036 //const int wheel = 257; //in mm
00037 //int Tdist = 0;
00038 
00039 //intrupt program exicuted
00040 void trigger() {
00041 
00042     end = timer.read_ms(); //read timer
00043     count = count + 1; //add to total turn count
00044     single_spin = end - begin; //calculate total time for 1 spin
00045 
00046     FILE *fp = fopen("/local/out.csv", "a"); //open file in append
00047     fprintf(fp, "%d, %d\n", count, single_spin);// add data to file
00048     fclose(fp); //colse file
00049 
00050     begin = timer.read_ms(); // reset timer
00051 
00052 
00053 }
00054 
00055 
00056 int main() {
00057 
00058     timer.start(); //start timer
00059     w.rise(&trigger); //set trigger
00060 
00061     b = 0;
00062     reset();
00063 
00064     //main loop
00065     while (1) {
00066 
00067         //init
00068         i = front.read();
00069         j = back.read();
00070 
00071 
00072         //can go forward?
00073         if (i > 100) {
00074             while (i > 100) {
00075 
00076                 forward(20);
00077                 i = front.read();
00078                 j = back.read();
00079             }
00080 
00081             stop();
00082         }
00083 
00084         //can it turn to avoid?
00085         if (i > 10 && i < 100) {
00086             led1 = 1;
00087             while (i > 10 && i < 100) {
00088 
00089                 forward(20);
00090                 turn.write(0);
00091 
00092                 i = front.read();
00093                 j = back.read();
00094             }
00095             led1 = 0;
00096             stop();
00097         }
00098 
00099         //is there room to ververse and turn?
00100         if (i < 20 && j > 10) {
00101             led2 = 1;
00102             while (i < 20 && j > 10) {
00103 
00104                 backward(20);
00105                 turn.write(1);
00106 
00107                 i = front.read();
00108                 j = back.read();
00109             }
00110             led2 = 0;
00111             stop();
00112         }
00113     }
00114 }
00115 
00116 void reset (void) {
00117     turn.write(0.5);
00118     motor.write(0.48);
00119 }
00120 
00121 // 0.5 = 0 speed & 0 = full speed
00122 void forward (float speed) {
00123     float y = speed / 100;
00124     float x = y * 0.5;
00125     float z = 0.5 - x;
00126 
00127     motor.write(z);
00128 }
00129 
00130 // 0.5 = 0 speed & 1 = full speed
00131 
00132 void backward (float speed) {
00133     float y = speed / 100;
00134     float x = y * 0.5;
00135     float z = 0.5 + x;
00136 
00137     motor.write(z);
00138 }
00139 
00140 // 0 = right 0.5 = straight 1 = left
00141 void left (void) {
00142     turn.write(1);
00143 }
00144 void right (void) {
00145     turn.write(0);
00146 }
00147 
00148 void stop (void) {
00149     b = 1;
00150     wait(0.3);
00151     reset();
00152     wait(0.1);
00153     b = 0;
00154     wait(1);
00155 }