main.cpp

00001 #include "mbed.h"
00002 #include "variables.h"
00003 #include "SRF05.h"
00004 
00005 DigitalOut myled(LED1);
00006 #include <vector> //Tracker data
00007 
00008 
00009 #define TESTING 1  //for TDD
00010 
00011 
00012 // Forwards and backwards
00013 void driveWheels(int direction, float pause)
00014 {
00015     
00016 #if TESTING
00017     pc.printf("Driving\n");
00018 #endif
00019     
00020     if(direction == forward)
00021     {
00022         drive   = 1;
00023         turnRight     = 0;
00024         wait(pause);        
00025     }
00026     else
00027     {
00028         drive   = 0;
00029         turnRight     = 1;
00030         wait(pause);  
00031     }
00032     drive   = 0;
00033     turnRight     = 0;
00034     
00035 #if TESTING
00036     wait(0.01);
00037 #endif
00038 }
00039 
00040 // Steering function
00041 void turnWheels(int direction)
00042 {
00043 #if TESTING
00044     pc.printf("Turning\n");
00045 #endif
00046     switch(direction)
00047     {
00048         case left:
00049             turnLeft = 0;
00050             turnRight = 1;
00051             break;
00052         case right:
00053             turnLeft = 1;
00054             turnRight = 0;
00055             break;  
00056         case center:
00057         defauturnLeft:
00058             turnLeft = 0;
00059             turnRight = 0;
00060             break;
00061     }
00062 }
00063 
00064 //Robot location
00065 std::vector <float> readPos()
00066 {
00067 #if TESTING
00068     pc.printf("Reading\n");
00069 #endif
00070     std::vector<float> pos;
00071     pos.push_back(ir1.read());
00072     pos.push_back(ir2.read());
00073     pos.push_back(ir3.read());
00074     pos.push_back(ir4.read());
00075     pos.push_back(ir5.read());
00076     return pos;
00077 }
00078 
00079 int wheelDir(std::vector<float> line)
00080 {
00081 #if TESTING
00082     pc.printf("Locating\n");
00083 #endif
00084     int ret = 100;
00085     float low = 1;
00086     for (unsigned i=0; i<line.size(); ++i)
00087     {
00088 #if TESTING
00089     pc.printf("%.1f", line[i]);
00090     pc.printf("\n");
00091 #endif
00092         if(line[i] < low && line[i] < 0.7)
00093         {
00094             ret = i; 
00095             low = line[i];
00096         }
00097        
00098     }
00099         
00100 #if TESTING
00101     pc.printf("%u", ret);
00102     pc.printf("\n");
00103 #endif
00104     return ret;
00105 }
00106 //Collision handling
00107 float collision(float col){
00108     col = srf.read();
00109 #if TESTING    
00110     pc.printf("Distance = %.1f\n", col);
00111 #endif    
00112     return col;
00113 }
00114 
00115 //Main Loop
00116 int main()
00117 {
00118 #if TESTING
00119     pc.printf("Set_up_variables\n");
00120 #endif
00121     std::vector <float>line;
00122     std::vector <float> pturnRightLine = readPos();
00123     int dir;
00124     int pos;
00125     float col;
00126     while(1)
00127     {
00128 #if TESTING
00129         pc.printf("Main\n");
00130 #endif
00131         line = readPos();
00132         pos = wheelDir(line);
00133 #if TESTING
00134         pc.printf("%u",pos);
00135 #endif
00136         
00137         if(pos == 100) 
00138         { 
00139             driveWheels(backward, fulldrive);
00140         }
00141         else 
00142         {
00143             if(pturnRightLine != line)
00144             {
00145                 switch(pos) 
00146                 {
00147                     case 0:
00148                     case 1:
00149                         dir = left;
00150                         break; 
00151                     case 2: 
00152                         dir = center;
00153                         break;
00154                     case 3:
00155                     case 4:
00156                         dir = right;
00157                         break;   
00158                 }
00159                 pturnRightLine.clear();
00160                 std::vector<float> pturnRightLine(line);     
00161                 turnWheels(dir);
00162                 collision(col);    
00163             }
00164             driveWheels(forward, fulldrive);
00165             //Collision handling
00166             if(col <= 200){
00167                 driveWheels(backward, fulldrive);
00168             }      
00169         } 
00170     }
00171 }