main.cpp

// 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);

//Return direction from servo - left, right or front
enum DIRC{LEFT,RIGHT,FRONT};

//Control Motor to turn right.
void TurnRight()
{
    
      ML.speed(-0.3);
      MR.speed(0.3);
      wait(2.1);
     
}    

//Control Motor to turn left. 
void TurnLeft()
{
    
      ML.speed(0.3);
      MR.speed(-0.3);
      wait(2.1);
     
}    

//Control Motor to go forward.
void Forward()
{
    ML.speed(-0.55);
    MR.speed(-0.5);
    wait(0.01);
}

//Control Motor to Backup.
 void Backup()
{
     ML.speed(0.55);
     MR.speed(0.5);
}

//Control the servo to TurnLeft.
DIRC LookLeft()
{
    myservo = 0.0;
    wait(1);
    return LEFT;
}

//Control the servo to TurnRight.
DIRC LookRight()
{
    myservo = 0.9;
    wait(1);
    return RIGHT;
}

//Control the servo to LookStraight.
DIRC LookStraight()
{
    myservo = 0.5;
    wait(1);
    return FRONT;
}

int main() {
         
     STBY = 1;//enable both Motor
     wait(1.0);
     DIRC looking_dir;
    
    while(true)
    {
        Forward(); //Constantly go straight if there is no obstacle.

        
        while(1)//Control servo swing left and right, obtain IR value.
        {
            looking_dir = LookStraight();
            if(IR>0.6) break;
            looking_dir = LookLeft();
            if(IR>0.6) break;
            looking_dir = LookRight();
            if(IR>0.6) break;

        }
        
        //Base on IR value, make correct movement. 
        if(looking_dir == FRONT)
        {
            Backup();
            wait(2.0);
            TurnRight();
        }
        else if(looking_dir == LEFT)
        {
            Backup();
            wait(2.0);
            TurnRight();
        }
        else
        {
            Backup();
            wait(2.0);
            TurnLeft();
        }
    }
}