File content as of revision 1:43568a9d4323:

#include "mbed.h"
#include "hcsr04.h"
#include "Servo.h"
#include "string.h"
#define HIGH 1
#define LOW 0

void forward();
void backward();
void right();
void left();
void stop();
void loop();

void blueInterrupt();
void timerInterrupt();
void bluecheck();

int distance;                 //Variable to store distance from an object
int checkRight;
int checkLeft;
int function=1;               //Variable to store function of robot: '1' running or '0' stoped. By 
int pos=90; 
int flagBlue=0;
int flagTimer=0;

Ticker timer;


int  c;


Serial pc(USBTX,USBRX);
Serial blue(PTC15,PTC14);
PwmOut servo(D6);    

char send[512],rcv[1000];//Variables

int main()
{
     
     blue.baud(9600);
     
 while(1)
 {        
     if(!blue.readable())
     {
            !blue.gets(rcv,1000);
            if(strstr(rcv,"forward")!=NULL)
            {
                forward();
                }
            else if(strstr(rcv,"backward")!=NULL)
            {
                backward();
                }
             else if(strstr(rcv,"right")!=NULL)
            {
                right();
                }
                else if(strstr(rcv,"left")!=NULL)
                {
                    left();     
                    }                 
              }                          
         else                
                {                                        
                    stop();                    
                    }   
}
}   
    
void forward(){

    

    pc.printf("fwd");

    DigitalOut(D3,1);

    DigitalOut(D5, 0);

    DigitalOut(D10, 1);

    DigitalOut(D11, 0); 

}

void backward(){

    DigitalOut(D3,0);

    DigitalOut(D5, 1);

    DigitalOut(D10, 0);

    DigitalOut(D11, 1); 

}


void right()


{
  

    DigitalOut(D3,0);

    DigitalOut(D5, 1);

    DigitalOut(D10, 1);

    DigitalOut(D11, 0); 


    }

   
    void left()


{

    DigitalOut(D3,1);

    DigitalOut(D5, 0);

    DigitalOut(D10, 0);

    DigitalOut(D11, 1); 

    
    }



  void stop()


{


    DigitalOut(D3,0);

    DigitalOut(D5, 0);

    DigitalOut(D10,0);

    DigitalOut(D11, 0); 


    }
    
  /*  
    void loop()
    
    {
        
           servo.period(0.020);
            usensor.start();
        wait_ms(500); 
        distance = usensor.get_dist_cm(); //Tip: Use 'CM' for centimeters or 'INC' for inches
        pc.printf("Dist: %d ",distance);
        
        //Check for objects...
        if (distance > 50){
            pc.printf(" No Object \n");
            forward(); //All clear, move forward!
            //noTone(buzzer);
           // digitalOut LED(0);
        }
        else if (distance <=50){
            pc.printf("Halt - Object on the Way \n");
            stop();
            

               for(float offset=0.0016; offset<=0.0020; offset+=0.0001) // turning RIGHT

{
    
      pc.printf("Turning Right"); 

servo.pulsewidth(offset); // servo position determined by a pulsewidth between

wait_ms(0.10);

}

wait_ms(200); // stopping at RIGHTMOST position for 2 seconds

    
        usensor.start();

        wait_ms(500); 

            checkRight = usensor.get_dist_cm();      //Take distance from the left side

            pc.printf("DistL: %d ",checkRight); 
            
      
            for (float offset=0.0020; offset>=0.0016;) // turning back to theCENTER position
{
 pc.printf("Turning Center"); 

servo.pulsewidth(offset); // servo position determined by a pulsewidth between
offset=offset-0.0001;

wait_ms(0.10);

}

wait_ms(200);

            
for (float offset=0.0016; offset>=0.0012; offset=offset-0.0001) // Turning towards LEFT

{

 pc.printf("Turning Left"); 
servo.pulsewidth(offset); // servo position determined by a pulsewidth between

wait_ms(0.10);

}

wait_ms(200); // stopping at LEFTMOST position for 2
            

        usensor.start();

        wait_ms(500); 

            checkLeft= usensor.get_dist_cm();

            pc.printf("DistR: %d ",checkLeft); 

            
for(float offset=0.0012; offset<=0.0016; offset=offset+0.0001)

{
    pc.printf("Turning Center"); 

servo.pulsewidth(offset); // servo position determined by a pulsewidth between

wait_ms(0.10);

}
            

            //Finally, take the right decision, turn left or right?

            if (checkLeft < checkRight){
                
                 backward();
                   wait_ms(1000);


                left();
pc.printf("goingleft");
                wait_ms(400);
                
                 // wait_ms, change value if necessary to make robot turn.            
              forward();
              pc.printf("now going straight");
                }

            else if (checkLeft > checkRight){
                
                backward();
                   wait_ms(1000);

                right();
            
            

                wait_ms(400);
                forward();
                 // wait_ms, change value if necessary to make robot turn.

            }

            else if (checkLeft <=10 && checkRight <=10){

                backward();
                 wait_ms(1000); //The road is closed... go back and then left ;)

                left();
                 wait_ms(400);
                forward();

            }

        }
*/