File content as of revision 2:26960ab3c751:

// Ojbective:
// Sweep servo fullrange from left to right (with left having the priority) to determine the next move.
// Robot will not move to obstacle closer than 10 cm, turn away instead

#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

// IR reading of 10 cm converted to 0-1 scale
#define WALL 0.7

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
Servo myservo(p21);//PWM control for Servo
AnalogIn IR(p20); //IR sensor
DigitalOut STBY(p11); // Set STBY = 1, enable both motor; Set STBY = 0; disable both motor. 
AnalogIn LeftEncoder(p19);
AnalogIn RightEncoder(p18);

// Adjust wheel speed using rotary magnetic encoder
void Adjust()
{
       //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);
        float left = LeftEncoder.read();
        float right = RightEncoder.read();
        float diff = abs(left - right);
        pc.printf("left: %.2f, right: %.2f, diff: %.2f\n", left, right, diff);
        //TO DO: Utilize diff data to adjust speeds of two wheels
}

void TurnRight(float deg)
{
    imu.readMag();
    float current_deg = imu.calcHeading();
    if (current_deg >= 270)
    {
        current_deg -= 360;
    }
    //pc.printf("Initial degree: %.2f  ", current_deg);
    float target_deg = current_deg + deg;
    //pc.printf("\nTarget degree: %.2f\n", target_deg);
    while(current_deg < target_deg)
    {
        ML.speed(-0.5);
        MR.speed(0.5);
        wait(0.001);
        // update
        imu.readMag();
        current_deg = imu.calcHeading();
        if (current_deg >= 270)
        {
            current_deg -=360;
        }
        //pc.printf("Current degree: %.2f  ", current_deg);
    }
    ML.speed(0.0);
    MR.speed(0.0);
}    

void TurnLeft(float deg)
{
    imu.readMag();
    float current_deg = imu.calcHeading();
    if (current_deg <= 90)
    {
        current_deg += 360;
    }
    pc.printf("Initial degree: %.2f  ", current_deg);
    float target_deg = current_deg - deg;
    pc.printf("\nTarget degree: %.2f\n", target_deg);
    while(current_deg > target_deg)
    {
        ML.speed(0.5);
        MR.speed(-0.5);
        wait(0.001);
        // update
        imu.readMag();
        current_deg = imu.calcHeading();
        if (current_deg <= 90)
        {
            current_deg += 360;
        }
        //pc.printf("Current degree: %.2f  ", current_deg);
    }
    ML.speed(0.0);
    MR.speed(0.0);
}    

void Backout()
{
    ML.speed(-0.5);
    MR.speed(-0.5);
    Adjust();
    wait(5.0);
}

void Forward()
{
    ML.speed(0.5);
    MR.speed(0.5);
    Adjust();
    wait(5.0);
}

void LookLeft()
{
    myservo = myservo - 0.1;   
}

void LookRight()
{
    myservo = myservo + 0.1;
}

void LookStraight()
{
    myservo = 0.5;
}

int main() {
    //float deg;
    STBY = 1;
    pc.baud(9600);
    uint16_t status = imu.begin();
    pc.printf("LSM9DS0 WHO_AM_I's returned: 0x%X\n", status);
    pc.printf("Should be 0x49D4\n\n");
    LookStraight();
    wait(2.0);
    //while(true)
//    {
//        TurnLeft(20.0);
//    } // testing
    /*
    while(true)
    {
        if(IR >= WALL)
        {
            if (myservo <= 0.5 && myservo > 0.0)
            { 
                LookLeft();
                wait(1.0);
            } // Look further left
            if (myservo > 0.5 && myservo < 1.0)
            {
                LookRight();
                wait(1.0);
            } // Look further right
            if (myservo == 0.0)
            {
                LookStraight();
                LookRight();
                wait(1.0);
            } // When reaches leftmost bound, reset to middle and start to look right
            if (myservo == 1.0)
            {
                Backout();
            } // When reaches rightmost bound, ie. sweeped the fullrange, backout
            else
            {
                pc.printf("servo error.");
            }
            wait(5.0);
        }
        else
        {
            // myservo has a range of 0.0 - 1.0 
            if (myservo > 0.5)
            {
               deg = (myservo - 0.5) * 360;
               TurnRight(deg);
            }
            else
            {
                deg = (0.5 - myservo) * 360;
                TurnLeft(deg);
            }
            LookStraight();
            Forward();
            wait(1.0);
        }
    }
    */
}