main.cpp

00001 #include "mbed.h"
00002 #include "Servo.h"
00003 #include "SRF05.h"
00004 #include "VS1002.h"
00005 #include "Serializer.h"
00006 #include "CMPS03.h"
00007 #include "math.h"
00008 
00009 SRF05 srf1(p22, p23);
00010 SRF05 srf2(p24, p25);
00011 Serial pc(USBTX, USBRX);
00012 Servo myservo(p21);
00013 VS1002 mp3(p5, p6, p7, p8, "sd",p11, p12, p13, p14, p15,p16, p17, p20);     //p14 in BoB non-functional so replace with p18
00014 Serializer robot;
00015 CMPS03 compass(p9,p10, CMPS03_DEFAULT_I2C_ADDRESS);
00016 
00017 
00018 float motor_turn();
00019 void deviate_right(float angle);
00020 void deviate_left(float angle);
00021 int free_space_found = 0;
00022 int counter =1;
00023 
00024 int main()
00025 {
00026    
00027     float angle = 0;
00028     float distance1 = 200;
00029     float distance2 = 0;
00030     int obstacle = 0;
00031     int obstacle_count = 0;
00032     myservo.calibrate(.0009, 180);
00033    
00034     #ifndef FS_ONLY   
00035     mp3._RST = 1;
00036     mp3.cs_high();                                   //chip disabled
00037     mp3.sci_initialise();
00038     pc.printf( "\n\rInitializing SCI!");
00039                            //initialise MBED
00040     mp3.sci_write(0x00,(SM_SDINEW+SM_STREAM+SM_DIFF));
00041     pc.printf( "\n\rWrite1!");
00042 
00043     mp3.sci_write(0x03, 0x9800);
00044     pc.printf( "\n\rWrite2!");
00045 
00046     mp3.sdi_initialise();   
00047     pc.printf("\n\rInitialing SDI!");
00048 
00049 #endif        
00050     while(1)   
00051     {
00052         pc.printf("\n\rNew code!!!");
00053         myservo = 0.5;
00054         wait(1);
00055         distance1 = srf1.read();
00056       //  distance2 = 200;
00057       //distance1 = 200;
00058         distance2 = srf2.read();
00059         pc.printf("\n\rDistance1:%f", distance1);
00060         pc.printf("\n\rDistance2:%f", distance2);
00061         if(distance1 > 100 && distance2 > 120 )
00062         {
00063             pc.printf("\n\r No obstacle! Go straight");
00064             obstacle = 0;
00065             robot.SetSpeed(70);
00066            
00067         }
00068         else
00069         {
00070             obstacle = 1;
00071             obstacle_count++;   
00072            // robot.Stop();
00073         }
00074        
00075         if(obstacle_count == 1)
00076         {
00077             obstacle_count = 0;
00078             robot.Stop();
00079             mp3.play_song(11);
00080             angle = motor_turn();
00081             if( angle == -1000 )
00082             {
00083                 pc.printf("\n\rStuck!");
00084               //  robot.SetSpeed(-70);
00085               //go back
00086                
00087             }
00088             else if (angle > 0)
00089             {
00090                 pc.printf("\n\rCalling deviate_right(%f)", angle);
00091                 deviate_right(angle);
00092                 robot.SetSpeed(70);
00093               // robot.PivetRight(angle);
00094             }
00095             else if(angle < 0 )
00096             {
00097                 pc.printf("\n\rCalling deviate_left(%f)", angle);
00098                 deviate_left(angle);
00099                 robot.SetSpeed(70);
00100              //robot.PivetLeft(abs (angle));
00101              }
00102              else if (angle == 0)
00103              { 
00104                  pc.printf("\n\rContine going straight!");        
00105                  robot.SetSpeed(70);
00106              }
00107         }
00108       
00109     }
00110 }
00111 
00112 
00113 float motor_turn()
00114 {
00115    free_space_found = 0;
00116    counter = 1;
00117     float distance1 = 0;
00118     float distance2 = 0;
00119     for(float i = 0.5; i <= 1; i=i+0.16)
00120     {
00121         myservo = i;
00122         wait(1);
00123         distance1 = srf1.read();
00124         distance2 = srf2.read();
00125         pc.printf("\n\rDistance1:%f", distance1);
00126         pc.printf("\n\rDistance2:%f", distance2);
00127         pc.printf("\n\rAngle:%f" , (90+180*(i-0.5)));
00128         pc.printf("\n\ri:%f", i);
00129        
00130         if( distance1 > 100 && distance2 > 120)
00131         {
00132             pc.printf("\n\rFree space found at %f degree!", (90+180*(i-0.5)));
00133             free_space_found = 1;
00134             mp3.play_song(counter);
00135             return (180*(i-0.5));
00136           
00137         }
00138         counter++;
00139     }
00140     if( free_space_found == 0 )
00141     {
00142         pc.printf("\n\rNo free space on right side!!");
00143          for(float i = 0.5; i >= 0 ; i=i-0.16)
00144          {
00145             myservo = i;
00146             wait(1);
00147              distance1 = srf1.read();
00148             distance2 = srf2.read();
00149             pc.printf("\n\rDistance1:%f", distance1);
00150             pc.printf("\n\rDistance2:%f", distance2);
00151               pc.printf("\n\ri:%f", i);
00152             pc.printf("\n\rAngle:%f" , (90+180*(i-0.5)));
00153             if( distance1 > 100 && distance2 > 120 )
00154             {
00155                pc.printf("\n\rFree space found at %f degree!", (90+180*(i-0.5)));
00156                free_space_found = 1;
00157                 mp3.play_song(counter);
00158                return (180*(i-0.5));
00159               
00160             }
00161             counter++;
00162          }
00163     }
00164    
00165     if( free_space_found == 0)
00166     {
00167         pc.printf("\n\rNo free space found! Go back!");
00168         mp3.play_song(10);
00169         return(-1000);
00170     }
00171     return 0.0;
00172 }
00173 
00174 
00175 void deviate_right(float angle)
00176 {
00177     float distance1 = 0;
00178     float distance2 = 0;
00179     float distance3 = 0;
00180     float distance4 =0;
00181     float i = 0.5;
00182     float deviation = 0;//Check for float with pavel
00183     int count_deviation = 0;
00184     pc.printf("\n\rInside deviate_right()");
00185     robot.PivetRight(angle);
00186     wait(2);
00187     pc.printf("\n\rPivetRight(%f)",angle);
00188     robot.ClearCount();
00189     pc.printf("\n\rClear count!");
00190     robot.SetSpeed(70);
00191     pc.printf("\n\rSetSpeed(70)");
00192     while(1)
00193     {
00194         pc.printf("\n\rInside while!!!");
00195         count_deviation = 0;
00196         myservo = 0.5;
00197         wait(1);
00198         distance1 = srf1.read();
00199         distance2 = srf2.read();
00200         i = (-1*angle/180 + 0.5);
00201         myservo = i;
00202         wait(1);
00203         distance3 = srf1.read();
00204         distance4 = srf2.read();
00205        
00206         if(distance3 > 100 && distance4 > 120)
00207         {
00208             pc.printf("\n\rFree space at 2*i");
00209             deviation = robot.GetDistance();
00210             count_deviation = robot.GetCount();
00211             pc.printf("\n\rDeviation: %f, Count_deviation:%d", deviation, count_deviation);
00212             robot.Stop();
00213             mp3.play_song(11);
00214             pc.printf("\n\rStop!");
00215             robot.PivetLeft(angle);
00216             mp3.play_song(counter+4);
00217             wait(2);
00218             robot.SetSpeed(70);
00219             pc.printf("\n\rPivetleft(2*%f)", angle);
00220         //    robot.DiGo(count_deviation, 70); //assuming that there is no obstacle in return path!
00221           //  pc.printf("\n\rDiGo(%d,70)", count_deviation);
00222           /*  while(count_deviation != 0 )
00223             {
00224                 pc.printf("\n\rcount_deviation:%d", count_deviation);
00225                 count_deviation --;
00226                 wait(0.01);
00227             }*/
00228            // wait(5);   
00229            // robot.Stop();//??
00230            // pc.printf("\n\rStop!");
00231            // robot.PivetRight(angle);
00232            // pc.printf("\n\rPivetRight(%f)", angle);
00233             return;      
00234         }
00235         if( distance1 < 100 || distance2 < 120 ) //changed!
00236         {
00237             pc.printf("\n\rStuck!");
00238             robot.Stop();
00239              mp3.play_song(11);
00240             //goback
00241         }
00242            
00243     }
00244 }
00245 
00246 
00247 void deviate_left(float angle)
00248 {
00249     float distance1 = 0;
00250     float distance2 = 0;
00251     float distance3 = 0;
00252     float distance4 = 0;
00253     float i = 0.5;
00254     float deviation = 0;//Check for float with pavel
00255     int count_deviation = 0;
00256     robot.PivetLeft(-angle);
00257     wait(2);
00258     pc.printf("\n\rInside deviate_left!!");
00259     pc.printf("\n\rPivetLeft(%f)", -angle);
00260     robot.ClearCount();
00261     pc.printf("\n\rClearCount");
00262     robot.SetSpeed(70);
00263     pc.printf("\n\rSetSpeed(70)");
00264     while(1)
00265     {
00266         count_deviation = 0;
00267         myservo = 0.5;
00268         wait(1);
00269         distance1 = srf1.read();
00270         distance2 = srf2.read();
00271         i = (-1*angle/180 + 0.5);
00272         myservo = i;
00273         wait(1);
00274         distance3 = srf1.read();
00275         distance4 = srf2.read();
00276         if(distance3 > 100 && distance4 > 120)
00277         {
00278             pc.printf("\n\r Free space at 2*i!");
00279             deviation = robot.GetDistance();
00280             count_deviation = robot.GetCount();
00281             pc.printf("\n\rDeviation:%f, count_deviation:%d", deviation, count_deviation);
00282             robot.Stop();
00283              mp3.play_song(11);
00284             pc.printf("\n\rStop!");
00285             robot.PivetRight(-1*angle);
00286              mp3.play_song(counter-4);
00287             wait(2);
00288             pc.printf("\n\rPivetRight(2*%f)", -angle);
00289             robot.SetSpeed(70);
00290           //  robot.DiGo(count_deviation, 70); //assuming that there is no obstacle in return path!
00291          //   pc.printf("\n\rDigo(%d,70)", count_deviation);
00292          /*   while(count_deviation != 0 )
00293             {
00294                 pc.printf("\n\rcount_deviation:%d",count_deviation);
00295                 count_deviation --;
00296                 wait(0.01);
00297             }*/
00298          //   wait(5);
00299           //  robot.Stop();//??
00300           ////  pc.printf("\n\rStop!");
00301           //  robot.PivetLeft(-angle);
00302           //  wait(5);
00303           //  pc.printf("\n\rPivetLeft(%f)", -angle);
00304             return;      
00305         }
00306         if( distance1 < 100 || distance2 < 120 ) //changed
00307         {
00308             pc.printf("\n\rStuck!");
00309             robot.Stop();
00310              mp3.play_song(11);
00311             float ang = 0;
00312            // ang = motor_turn();
00313            
00314            
00315             //goback
00316         }    
00317     }
00318 
00319 }