main.cpp

00001 #include "mbed.h"
00002 #include "m3pi_ng.h"
00003 #include "DigitalIn.h"
00004 #include "btbee.h"
00005 #include "time.h"
00006 
00007 
00008 DigitalOut myled(LED1);
00009 m3pi huey;
00010 btbee btbee;
00011 
00012 DigitalIn m3pi_IN[]={(p12),(p21)}; // IR-Sensor
00013 DigitalOut mbed_led[] = {(LED1), (LED2),(LED3), (LED4)};
00014 DigitalOut m3pi_led[] = {(p13), (p14), (p15), (p16), (p17), (p18), (p19), (p20)};
00015 DigitalIn m3pi_pb(p21);
00016 
00017 void printBattery()
00018 {
00019     //prints battery voltage on led screen
00020     float bat = huey.battery();
00021     huey.printf("Bat volt");
00022     huey.locate(0,1);
00023     huey.printf("%f",bat); 
00024 }
00025 
00026 float myLinePos(void)
00027 {
00028     int sensors[5];
00029     huey.calibrated_sensor(sensors);
00030     //oat leftSen, rightSen, midSen;
00031     //ftSen = sensors[1] / 1000;
00032     //ghtSen = sensors[3] / 1000;
00033     //dSen = sensors[2] / 1000;
00034     float val;
00035     if(sensors[1] < 50 && sensors[2] < 50 && sensors[3] < 50)
00036         return -1;
00037     else
00038     {
00039         val = (((1000*sensors[2] + 2000*sensors[3])/(sensors[1]+sensors[2]+sensors[3])));
00040         return (-1)+(val/1000);
00041     }
00042 }
00043 
00044 int rightCorner()
00045 {
00046     int sensors[5];
00047     huey.calibrated_sensor(sensors);
00048     if(sensors[4] > 950)
00049         return 1;
00050     else
00051         return 0;
00052 }
00053 
00054 int leftCorner()
00055 {
00056     int sensors[5];
00057     huey.calibrated_sensor(sensors);
00058     if(sensors[0] > 950)
00059         return 1;
00060     else
00061         return 0;
00062 }
00063 
00064 //returns 0 if side sensors aren't light, 1 if light
00065 int sideSensorsLight(void)
00066 {
00067     int sensors[5];
00068     huey.calibrated_sensor(sensors);
00069     if(sensors[4] > 50 && sensors[0] > 50)
00070         return 0;
00071     else
00072         return 1;
00073 }
00074 
00075 //returns 0 if side sensors aren't dark, 1 if dark
00076 int sideSensorsDark(void)
00077 {
00078     if(rightCorner() == 1 && leftCorner() == 1)
00079         return 1;
00080     else
00081         return 0; 
00082 }
00083 
00084 void rightTurn(float speed)
00085 {
00086     int side = sideSensorsLight();
00087     huey.forward(speed/2);
00088     while(side == 0)
00089     {
00090         side = sideSensorsLight();
00091     }
00092     wait(0.20);
00093     float pos = myLinePos();
00094     huey.right(speed/2);
00095     while(pos > -0.1 && pos < 0.1)
00096     {
00097          pos = myLinePos();
00098     }
00099     pos = myLinePos();
00100     while(pos > 0.05 || pos < -0.05)
00101     {    
00102         pos = myLinePos();
00103     } 
00104 }
00105 
00106 void leftTurn(float speed)
00107 {
00108     int side = sideSensorsLight();
00109     huey.forward(speed/2);
00110     while(side == 0)
00111     {
00112         side = sideSensorsLight(); 
00113     }
00114     wait(0.20);
00115             
00116     float pos = myLinePos();
00117     huey.left(speed/2);
00118     while(pos > -0.1 && pos < 0.1)
00119     {
00120         pos = myLinePos();
00121     }
00122     pos = myLinePos();
00123     while(pos > 0.05 || pos < -0.05)
00124     {    
00125         pos = myLinePos();
00126     }    
00127 }
00128 
00129 void turnAround(float speed)
00130 {
00131     float pos = myLinePos();
00132     huey.right(speed/2);
00133     while(pos > -0.1 && pos < 0.1)
00134     {
00135          pos = myLinePos();
00136     }
00137     pos = myLinePos();
00138     while(pos > 0.05 || pos < -0.05)
00139     {    
00140         pos = myLinePos();
00141     }
00142     while(pos > -0.1 && pos < 0.1)
00143     {
00144          pos = myLinePos();
00145     }
00146     pos = myLinePos();
00147     while(pos > 0.05 || pos < -0.05)
00148     {    
00149         pos = myLinePos();
00150     }  
00151 }
00152 
00153 //returns 1 for all turns, 0 for non turns, 2 for end of a task;
00154 int smoothFollow(float position, float speed, char direction)
00155 {
00156     float u = speed;
00157     u = u * position;
00158     
00159         if(rightCorner() == 1 && direction == 'R')
00160         {         
00161             rightTurn(speed);
00162             huey.printf("R");
00163             return 1;
00164         }
00165         else if(leftCorner() == 1 && direction == 'L')
00166         {
00167             leftTurn(speed);
00168             huey.printf("L");
00169             return 1;
00170         }
00171         else if(sideSensorsDark() == 1 && direction == 'E')
00172         {
00173             turnAround(speed);
00174             huey.stop();
00175             return 2;  
00176         }
00177         else if((rightCorner() == 1 || leftCorner() == 1) && direction == 'S')
00178         {
00179             int side = sideSensorsLight();
00180             huey.forward(speed/2);
00181             while(side == 0)
00182             {
00183                 side = sideSensorsLight(); 
00184             }
00185             return 1;
00186         }  
00187         else if(speed+u > 1)
00188         {
00189             huey.stop();
00190             huey.printf("Fast Er");
00191             return 0;
00192         }
00193         else
00194         {
00195             huey.right_motor(speed-u);
00196             huey.left_motor(speed+u);
00197             return 0;
00198         }
00199 }
00200 
00201 
00202 
00203 /*
00204 void slowStop(float speed, float waitTime, int steps)
00205 {
00206     float i;
00207     for(i = speed; i > 0; i = i - (speed/steps))
00208     {
00209         
00210         smoothFollow(huey.line_position(),i);
00211         wait(waitTime);      
00212     }
00213     huey.stop();
00214 }
00215 */
00216 
00217 int pushToStart(void)
00218 {
00219     DigitalIn button(p21);
00220     int value = button.read();
00221     return value;
00222 }
00223 
00224 
00225 
00226 /////////////////////////////////////////////////////////////////
00227 int main() {
00228     
00229     
00230     //Matrix of paths from one point to another: Right = R, Left = L, Straight = S
00231     char map[6][6][20] =  
00232     {
00233         {"", "LRLLRE", "LRLSSE", "LRLSRLLE", "LRRLSLRRRLE", "LRRLRE"},
00234         {"LRRLRE", "", "LLSE", "LLRLLE", "LSRLRRLE", "LRSLRE"},
00235         {"SSRLRE", "SRRE", "", "LLLE", "LLRSRLE", "SSSLRE"}, 
00236         {"RLSRLRE", "RLRRE", "RRE", "", "SSRLE", "SRLRLE"}, 
00237         {"RLLLRSRLLRE", "RLLRLSRE", "RLSLRE", "RLSSE", "", "RLLLRLE"}, 
00238         {"LRLLRE", "LRSLRE", "LRSSSE", "RLRLSE", "RLRRRLE", ""}   
00239     };
00240     
00241     int points[3] = {1,2,4};    
00242     
00243     
00244     
00245     //time_t startt, endt;
00246     //time(&startt);
00247     //clock_t clock;
00248     
00249      //btbee trial
00250     // initialization stuff ////////////////////////////////////////////////////////////////////////////////////////////////////
00251     huey.locate(0,1);
00252     btbee.reset();
00253     for (int i = 0; i <4; i++) {
00254         mbed_led[i] = 0;
00255     }
00256     for (int i = 0; i <8; i++) {
00257         m3pi_led[i]=0;
00258     }
00259     m3pi_pb.mode(PullUp); // expected would be 1 when pb is pressed, 0 when not, opposite is the case
00260 
00261     // end initialization stuff ////////////////////////////////////////////////////////////////////////////////////////////////
00262 
00263     huey.locate(0,0);
00264     huey.printf("B:%0.3f%V",huey.battery());
00265     wait(0.3);
00266     
00267     huey.locate(0,0);
00268     huey.printf("%s","btTest");
00269     huey.locate(0,1);
00270     huey.printf("%s","PBonLNK");
00271 
00272     // wait for the user to push P21, should be pressed when the bt link is established (green led "link")
00273     while(m3pi_pb) {
00274         m3pi_led[0]=!m3pi_led[0];
00275         wait(0.1);
00276     }
00277 
00278     int iline=1;
00279     char arr_read[30]; // this should be long enough to store any reply coming in over bt.
00280     int  chars_read;   // number of chars read in a bt reply
00281     
00282     while (iline < 3) {
00283         // this writes "Line 001\n" to "Line 005\n" and then "end\n" to the btbee
00284         if ( btbee.writeable() ) {
00285             if (iline==2) {
00286                 //btbee.printf("end\n");
00287                 iline++;
00288             }//if
00289             else {
00290                 if (iline <2){
00291                 //time(&endt);
00292                 //btbee.printf("%f seconds - ", clock);
00293                 //btbee.printf("Battery Voltage: %f\n", huey.battery());
00294                 m3pi_led[0]=0;
00295                 huey.locate(0,0);
00296                 huey.printf("Sent %0.3d",iline);
00297                 iline++;
00298                 }
00299             }//else
00300         }//if_write
00301 
00302         // check for answers after each write /not write
00303         while ( btbee.readable() ) {
00304             m3pi_led[7]=1;
00305             btbee.read_all(arr_read, 30, &chars_read );
00306             m3pi_led[6]=1;
00307             huey.locate(0,1);
00308             huey.print(arr_read,chars_read);
00309             m3pi_led[5]=1;
00310         }//while_readable
00311         wait(0.1);
00312     }//while_true
00313     
00314 //main
00315 //end of btbee trial
00316    
00317     
00318     
00319     m3pi_IN[0].mode(PullUp);
00320     m3pi_IN[1].mode(PullUp);
00321     
00322     int start;
00323     do 
00324     {
00325         start = pushToStart();  
00326     }while(start == 1);
00327     
00328     wait(1);
00329     //calibrates sensors
00330     huey.sensor_auto_calibrate();  
00331 
00332     printBattery();
00333     
00334     
00335     float speed = 0.25;
00336     float pos;
00337     int z=1;
00338 
00339     
00340     while(z==1)
00341     {
00342              
00343         
00344         huey.cls();
00345         for(int p = 1; p < sizeof(points); p ++)
00346         {
00347             int s = points[p-1] - 1;
00348             int f = points[p] - 1;
00349             for(int i = 0; i < sizeof(map[s][f]); i++)
00350             {  
00351                 int change = 0;
00352                 while(change == 0)
00353                 {
00354                     pos = myLinePos();
00355                     change = smoothFollow(pos, speed, map[s][f][i]);
00356                 }
00357                 if(change == 2)
00358                 {
00359                     huey.stop();
00360                     wait(1);
00361                     break;
00362                 }
00363             }
00364         } 
00365         if(m3pi_IN[0]==0)
00366         {
00367             //slowStop(speed, 0.05, 3);
00368             huey.stop();
00369             btbee.printf("Object in Front");
00370             while(m3pi_IN[0]==0)
00371             {
00372                 huey.stop();    
00373             }
00374         }
00375         if(pushToStart() == 0)
00376         {
00377                 btbee.printf("end\n");
00378                 break;
00379         }
00380     }
00381 
00382     
00383     
00384     
00385     
00386     
00387     while(1) 
00388     {
00389         myled = 1;
00390         wait(0.2);
00391         myled = 0;
00392         wait(0.2);
00393     }
00394 }
00395