File content as of revision 0:ee786e500a3c:

#include "mbed.h"
#include "Servo.h"
#include "SRF05.h"
#include "VS1002.h"
#include "Serializer.h"
#include "CMPS03.h"
#include "math.h"

SRF05 srf1(p22, p23);
SRF05 srf2(p24, p25);
Serial pc(USBTX, USBRX);
Servo myservo(p21);
VS1002 mp3(p5, p6, p7, p8, "sd",
        p11, p12, p13, p14, p15,
        p16, p17, p20);     //p14 in BoB non-functional so replace with p18
Serializer robot;
//CMPS03 compass(p9,p10, CMPS03_DEFAULT_I2C_ADDRESS);


float motor_turn();
void deviate_right(float angle);
void deviate_left(float angle);
int free_space_found = 0;
int counter =1;

int main()
{
   
    float angle = 0;
    float distance1 = 200;
    float distance2 = 0;
    int obstacle = 0;
    int obstacle_count = 0;
    myservo.calibrate(.0009, 180);
   
    #ifndef FS_ONLY   
    mp3._RST = 1;
    mp3.cs_high();                                   //chip disabled
    mp3.sci_initialise();
    pc.printf( "\n\rInitializing SCI!");
                           //initialise MBED
    mp3.sci_write(0x00,(SM_SDINEW+SM_STREAM+SM_DIFF));
    pc.printf( "\n\rWrite1!");

    mp3.sci_write(0x03, 0x9800);
    pc.printf( "\n\rWrite2!");

    mp3.sdi_initialise();   
    pc.printf("\n\rInitialing SDI!");

#endif        
    while(1)   
    {
        pc.printf("\n\rNew code!!!");
        myservo = 0.5;
        wait(1);
        //distance1 = srf1.read();
      //  distance2 = 200;
      distance1 = 200;
        distance2 = srf2.read();
        pc.printf("\n\rDistance1:%f", distance1);
        pc.printf("\n\rDistance2:%f", distance2);
        if(distance1 > 100 && distance2 > 120 )
        {
            pc.printf("\n\r No obstacle! Go straight");
            obstacle = 0;
            robot.SetSpeed(70);
           
        }
        else
        {
            obstacle = 1;
            obstacle_count++;   
           // robot.Stop();
        }
       
        if(obstacle_count == 1)
        {
            obstacle_count = 0;
            robot.Stop();
            mp3.play_song(11);
            angle = motor_turn();
            if( angle == -1000 )
            {
                pc.printf("\n\rStuck!");
              //  robot.SetSpeed(-70);
              //go back
               
            }
            else if (angle > 0)
            {
                pc.printf("\n\rCalling deviate_right(%f)", angle);
                deviate_right(angle);
                robot.SetSpeed(70);
              // robot.PivetRight(angle);
            }
            else if(angle < 0 )
            {
                pc.printf("\n\rCalling deviate_left(%f)", angle);
                deviate_left(angle);
                robot.SetSpeed(70);
             //robot.PivetLeft(abs (angle));
             }
             else if (angle == 0)
             { 
                 pc.printf("\n\rContine going straight!");        
                 robot.SetSpeed(70);
             }
        }
      
    }
}


float motor_turn()
{
   free_space_found = 0;
   counter = 1;
    float distance1 = 0;
    float distance2 = 0;
    for(float i = 0.5; i <= 1; i=i+0.16)
    {
        myservo = i;
        wait(1);
     //   distance1 = srf1.read();
        distance2 = srf2.read();
        pc.printf("\n\rDistance1:%f", distance1);
        pc.printf("\n\rDistance2:%f", distance2);
        pc.printf("\n\rAngle:%f" , (90+180*(i-0.5)));
        pc.printf("\n\ri:%f", i);
       
        if( distance1 > 100 && distance2 > 120)
        {
            pc.printf("\n\rFree space found at %f degree!", (90+180*(i-0.5)));
            free_space_found = 1;
            mp3.play_song(counter);
            return (180*(i-0.5));
          
        }
        counter++;
    }
    if( free_space_found == 0 )
    {
        pc.printf("\n\rNo free space on right side!!");
         for(float i = 0.5; i >= 0 ; i=i-0.16)
         {
            myservo = i;
            wait(1);
          //  distance1 = srf1.read();
            distance2 = srf2.read();
            pc.printf("\n\rDistance1:%f", distance1);
            pc.printf("\n\rDistance2:%f", distance2);
              pc.printf("\n\ri:%f", i);
            pc.printf("\n\rAngle:%f" , (90+180*(i-0.5)));
            if( distance1 > 100 && distance2 > 120 )
            {
               pc.printf("\n\rFree space found at %f degree!", (90+180*(i-0.5)));
               free_space_found = 1;
                mp3.play_song(counter);
               return (180*(i-0.5));
              
            }
            counter++;
         }
    }
   
    if( free_space_found == 0)
    {
        pc.printf("\n\rNo free space found! Go back!");
        mp3.play_song(10);
        return(-1000);
    }
    return 0.0;
}


void deviate_right(float angle)
{
    float distance1 = 0;
    float distance2 = 0;
    float distance3 = 0;
    float distance4 =0;
    float i = 0.5;
    float deviation = 0;//Check for float with pavel
    int count_deviation = 0;
    pc.printf("\n\rInside deviate_right()");
    robot.PivetRight(angle);
    wait(2);
    pc.printf("\n\rPivetRight(%f)",angle);
    robot.ClearCount();
    pc.printf("\n\rClear count!");
    robot.SetSpeed(70);
    pc.printf("\n\rSetSpeed(70)");
    while(1)
    {
        pc.printf("\n\rInside while!!!");
        count_deviation = 0;
        myservo = 0.5;
        wait(1);
        //distance1 = srf1.read();
        distance2 = srf2.read();
        i = (-1*angle/180 + 0.5);
        myservo = i;
        wait(1);
       // distance3 = srf1.read();
       distance4 = srf2.read();
       
        if(distance3 > 100 && distance4 > 120)
        {
            pc.printf("\n\rFree space at 2*i");
            deviation = robot.GetDistance();
            count_deviation = robot.GetCount();
            pc.printf("\n\rDeviation: %f, Count_deviation:%d", deviation, count_deviation);
            robot.Stop();
            mp3.play_song(11);
            pc.printf("\n\rStop!");
            robot.PivetLeft(angle);
            mp3.play_song(counter+4);
            wait(2);
            robot.SetSpeed(70);
            pc.printf("\n\rPivetleft(2*%f)", angle);
        //    robot.DiGo(count_deviation, 70); //assuming that there is no obstacle in return path!
          //  pc.printf("\n\rDiGo(%d,70)", count_deviation);
          /*  while(count_deviation != 0 )
            {
                pc.printf("\n\rcount_deviation:%d", count_deviation);
                count_deviation --;
                wait(0.01);
            }*/
           // wait(5);   
           // robot.Stop();//??
           // pc.printf("\n\rStop!");
           // robot.PivetRight(angle);
           // pc.printf("\n\rPivetRight(%f)", angle);
            return;      
        }
        if( distance1 < 100 || distance2 < 120 ) //changed!
        {
            pc.printf("\n\rStuck!");
            robot.Stop();
             mp3.play_song(11);
            //goback
        }
           
    }
}


void deviate_left(float angle)
{
    float distance1 = 0;
    float distance2 = 0;
    float distance3 = 0;
    float distance4 = 0;
    float i = 0.5;
    float deviation = 0;//Check for float with pavel
    int count_deviation = 0;
    robot.PivetLeft(-angle);
    wait(2);
    pc.printf("\n\rInside deviate_left!!");
    pc.printf("\n\rPivetLeft(%f)", -angle);
    robot.ClearCount();
    pc.printf("\n\rClearCount");
    robot.SetSpeed(70);
    pc.printf("\n\rSetSpeed(70)");
    while(1)
    {
        count_deviation = 0;
        myservo = 0.5;
        wait(1);
      //  distance1 = srf1.read();
        distance2 = srf2.read();
        i = (-1*angle/180 + 0.5);
        myservo = i;
        wait(1);
       // distance3 = srf1.read();
        distance4 = srf2.read();
        if(distance3 > 100 && distance4 > 120)
        {
            pc.printf("\n\r Free space at 2*i!");
            deviation = robot.GetDistance();
            count_deviation = robot.GetCount();
            pc.printf("\n\rDeviation:%f, count_deviation:%d", deviation, count_deviation);
            robot.Stop();
             mp3.play_song(11);
            pc.printf("\n\rStop!");
            robot.PivetRight(-1*angle);
             mp3.play_song(counter-4);
            wait(2);
            pc.printf("\n\rPivetRight(2*%f)", -angle);
            robot.SetSpeed(70);
          //  robot.DiGo(count_deviation, 70); //assuming that there is no obstacle in return path!
         //   pc.printf("\n\rDigo(%d,70)", count_deviation);
         /*   while(count_deviation != 0 )
            {
                pc.printf("\n\rcount_deviation:%d",count_deviation);
                count_deviation --;
                wait(0.01);
            }*/
         //   wait(5);
          //  robot.Stop();//??
          ////  pc.printf("\n\rStop!");
          //  robot.PivetLeft(-angle);
          //  wait(5);
          //  pc.printf("\n\rPivetLeft(%f)", -angle);
            return;      
        }
        if( distance1 < 100 || distance2 < 120 ) //changed
        {
            pc.printf("\n\rStuck!");
            robot.Stop();
             mp3.play_song(11);
            float ang = 0;
           // ang = motor_turn();
           
           
            //goback
        }    
    }

}