File content as of revision 0:8239206a2f26:

// Hello World to sweep a servo through its full range

#include "mbed.h"
#include "Servo.h"
#include "Motor.h"
#include "LSM9DS0.h"

// SDO_XM and SDO_G are pulled up, so our addresses are:
#define LSM9DS0_XM_ADDR  0x1D // Would be 0x1E if SDO_XM is LOW
#define LSM9DS0_G_ADDR   0x6B // Would be 0x6A if SDO_G is LOW

// refresh time. set to 500 for checking the Data on pc.
#define REFRESH_TIME_MS 500

LSM9DS0 imu(p9, p10, LSM9DS0_G_ADDR, LSM9DS0_XM_ADDR);//IMU, read the compass value. 
Serial pc(USBTX, USBRX);//Only use this during the testing phase for pc debug.
Motor MR(p25, p6, p5); // Motor A pwm, fwd, rev
Motor ML(p26, p7, p8);//Motor B pwm, fwd, rev
DigitalOut STBY(p11); // Set STBY = 1, enable both motor; Set STBY = 0; disable both motor. 
Servo myservo(p21);//PWM control for Servo
AnalogIn IR(p20); //IR sensor
AnalogIn LeftEncoder(p19);

void TurnRight()
{
    //imu.readMag();
    /*float current_dir = imu.calcHeading();
    if (current_dir>=270)
    {
        current_dir -=360;
    }
    
    float target_dir = current_dir + 90;
    */
    
      //pc.printf("%.2f",current_dir);
      //pc.printf("       Targe:    %.2f\n",target_dir);
      ML.speed(-0.3);
      MR.speed(0.3);
      //imu.readMag();
      
      //current_dir = imu.calcHeading();
      
      /*if (current_dir>=270)
      {
            current_dir -=360;
      }*/
      wait(2.1);
     
      ML.speed(0.0);
      MR.speed(0.0);
}    

void TurnLeft()
{
    
      ML.speed(0.3);
      MR.speed(-0.3);
    
      wait(2.1);
     
      ML.speed(0.0);
      MR.speed(0.0);
}    

void Straight()
{
    ML.speed(-0.5);
    MR.speed(-0.5);
    wait(0.01);
}

void LookLeft()
{
    myservo = 0.0;
    
}

void LookRight()
{
    myservo = 0.9;
}

void LookStraight()
{
    myservo = 0.5;
}

int main() {
         
     STBY = 1;
     pc.baud(9600);
    imu.begin();
    LookStraight();
    wait(2.0);
    while(true)
    {
        if(IR>0.6)
        {
            LookLeft();
            wait(2.0);
            if(IR >0.6);
                {
                    LookRight();
                    wait(2.0);
                }
        }
 //           Straight();

   //     wait(10.0);
     //   TurnRight();
       // wait(2.0);
        //TurnLeft();
        
        //LookLeft();
        //wait(2);
        //myservo =  0.5;
        //imu.readMag();
        //float heading_turn = imu.calcHeading();
        //float Turn_Right   = heading + 90;
        
      //if((heading_turn - Turn_Right)<0.1)
        //{
      //    MA.speed(1.0f); MB.speed(1.0f); 
        //  pc.printf("%.2f\n",LeftEncoder.read());
      //  wait(5);
      else
        LookStraight();
        wait(1.0);
    }

      
    
}