File content as of revision 0:31128b41490c:

#include "mbed.h"
#include "PinDetect.h"
#include "uLCD_4DGL.h"
#include "math.h"
#include "LSM9DS0.h"
#include "Motor.h"
#define PI 3.14159265
#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 part 2 and 50 for part 4
#define REFRESH_TIME_MS 500
Serial pc(USBTX, USBRX);
// Verify that the pin assignments below match your breadboard
LSM9DS0 imu(p9, p10, LSM9DS0_G_ADDR, LSM9DS0_XM_ADDR);
Motor mA(p21, p23, p24); // pwm, fwd, rev LEFT
Motor mB(p22, p25, p26); // pwm, fwd, rev RIGHT
//speed: 10.44 cm/s
// use class to setup pushbuttons pins
PinDetect pb1(p16);
PinDetect pb2(p17);
PinDetect pb3(p18);
PinDetect pb4(p19);

DigitalOut myLED1(LED1);
DigitalOut myLED2(LED2);
DigitalOut myLED3(LED3);
DigitalOut myLED4(LED4);
// use class to setup the  Color LCD
uLCD_4DGL uLCD(p28, p27, p29); // create a global uLCD object

//Timer class
Timer timer;

//Mode settting

enum Modetype { SETX = 0, SETY, GO };
Modetype mode1 = SETX;
int settingMode = 0;
//Variable
int xCoord = 0;
int yCoord = 0;
float distance = 0;
float angle = 0;
float seconds = 0;
int startDegree = 0;
int turningDegree = 0;
int finalDegree = 0;
int heading = 0;
int degree = 0;
int initialHeading = 0;
int state = 0;
int difference = 0;
void setup()
{
    uLCD.cls();
    uint16_t status = imu.begin();
//Make sure communication is working
}

//When pb1 push increment the coordinate
void pb1_hit_callback (void)
{
    if (mode1 == SETX)
        xCoord++;
    else if (mode1 == SETY)
        yCoord++;
    
    myLED1 = 1;
    myLED2 = 0;
    myLED3 = 0;
    myLED4 = 0;
}

//When pb2 push decrement the coordinate
void pb2_hit_callback (void)
{
    if (mode1 == SETX)
        xCoord--;
    else if (mode1 == SETY)
        yCoord--;
        
    myLED2 = 1;
    myLED1 = 0;
    myLED3 = 0;
    myLED4 = 0;
    
}

//When pb3 push increment the mode
void pb3_hit_callback (void)
{
    mode1 = static_cast<Modetype>((static_cast<int>(mode1) + 1) % 3);
    myLED3 = 1;
    myLED1 = 0;
    myLED2 = 0;
    myLED4 = 0;
}

//When pb4 push decrement the mode
void pb4_hit_callback (void)
{
    if (mode1 == SETY)
    {
        mode1 = SETX;   
    }
    myLED4 = 1;
    myLED1 = 0;
    myLED2 = 0;
    myLED3 = 0;
}

void turnRight(void)
{
    imu.readMag();
    myLED3 = 1;
    uLCD.locate(1,5);
    uLCD.printf("Turning Right \n");
    mA.speed(0.2);
    mB.speed(-0.2);
    heading = static_cast<int> (imu.calcHeading());
    uLCD.printf("H : %d \n",heading);
    degree =   heading - initialHeading;
    degree = turningDegree - degree;
    uLCD.printf("Degree = %d \n",degree);
    uLCD.printf("Heading = %d \n",heading);
}

void turnLeft(void)
{
    imu.readMag();
    myLED3 = 1;
    uLCD.locate(1,5);
    uLCD.printf("Turning Left \n");
    mA.speed(-0.2);
    mB.speed(0.2);
    heading = static_cast<int> (imu.calcHeading());
    uLCD.printf("H : %d \n",heading);
    degree =   heading - initialHeading;
    degree = turningDegree - degree;
    uLCD.printf("Degree = %d \n",degree);
    uLCD.printf("Heading = %d \n",heading);
}


int main() 
{
    uLCD.cls();
    // Use internal pullups for the three pushbuttons
    pb1.mode(PullUp);
    pb2.mode(PullUp);
    pb3.mode(PullUp);
    pb4.mode(PullUp);
    // Delay for initial pullup to take effect
    wait(.01);
    // Setup Interrupt callback functions for a pb hit
    pb1.attach_deasserted(&pb1_hit_callback);
    pb2.attach_deasserted(&pb2_hit_callback);
    pb3.attach_deasserted(&pb3_hit_callback);
    pb4.attach_deasserted(&pb4_hit_callback);
    // Start sampling pb inputs using interrupts
    pb1.setSampleFrequency();
    pb2.setSampleFrequency();
    pb3.setSampleFrequency();
    pb4.setSampleFrequency();
    mode1 = SETX;
    setup();
    
    xCoord = 0;
    yCoord = 0;
    state = 1;
        //Setting mode
        while(settingMode == 0)
        {
            uLCD.locate(1,2);
            if (mode1 == SETX)
            {
                uLCD.printf("Setting X coordinate");    
            }
            else if (mode1 == SETY)
            {
                uLCD.printf("Setting Y Coordinate");   
            }
            else if (mode1 == GO)
            {
                uLCD.printf("Running Now");   
            }    
            uLCD.locate(4,5);
            uLCD.printf("X-Coord: %d \n\r", xCoord);
            uLCD.locate(4,7);
            uLCD.printf("Y-Coord: %d \n\r", yCoord);            
            
            if (mode1 == GO)
            {
                distance =  xCoord*xCoord + yCoord*yCoord;
                distance = sqrt(distance);
                float totalDistance = distance * 30.48;
                seconds = totalDistance / 23.18 * 1000;
                imu.readMag();
                initialHeading = static_cast<int>(imu.calcHeading());
                angle = atan2 (static_cast<float> (yCoord),static_cast<float>(xCoord)) * 180 / PI;
                finalDegree = initialHeading + angle;
                uLCD.locate(4,9);
                uLCD.printf("Distance: %5.2F Feet \n\r", distance);
                uLCD.locate(4,11);
                uLCD.printf("Turning: %5.2F Degree \n\r", angle);
                
                settingMode = 1;
            }
                
        } //end first while
        wait(2);
        //Turning mode
        while(settingMode == 1)
        {
            uLCD.cls();
            uLCD.printf("Setting mode 1");
            imu.readMag();
            initialHeading = static_cast<int> (imu.calcHeading());
            uLCD.locate(1,3);
            uLCD.printf("Initial : %d \n",initialHeading);
            
            startDegree = heading;
            turningDegree = angle;
            degree =  static_cast<int> (imu.calcHeading()) - initialHeading;
            
           if (turningDegree > 0)
           {
                while (degree  > -2 && degree < 180)
                {              
                    turnRight();
                    wait(0.2);
                    mA.speed(0);
                    mB.speed(0);  
                } 
                settingMode = 2;

            }
            else
            {
                while (degree < 2 && degree > -180 )
                {
                    turnLeft();
                    wait(0.2);
                    mA.speed(0);
                    mB.speed(0);  
                }
                settingMode = 2;
            }                                                  
        }
        
            wait(2);
            imu.readMag();
            initialHeading = static_cast<int> (imu.calcHeading());
            timer.start();
            while(timer.read_ms() < static_cast<int>(seconds) && settingMode == 2)
            {
                uLCD.locate(1,7);
                uLCD.printf("Time : %d \n",timer.read_ms());
                mA.speed(0.4);
                mB.speed(0.4);
                imu.readMag();
                heading = static_cast<int> (imu.calcHeading());
                difference = heading - initialHeading;
                uLCD.locate(1,3);
                uLCD.printf("Heading : %d \n",heading);
                if( difference <-2)
                {
                            mA.speed(0.5);
                            mB.speed(0.4);
                            imu.readMag();
                            heading = static_cast<int> (imu.calcHeading());
                            difference = heading - finalDegree;
                            uLCD.locate(1,3);
                            uLCD.printf("Heading : %d \n",heading);
                }
                 if(difference >2)
                {
                            mA.speed(0.4);
                            mB.speed(0.5);
                            imu.readMag();
                            heading = static_cast<int> (imu.calcHeading());
                            difference = heading - finalDegree;
                            uLCD.locate(1,3);
                            uLCD.printf("Heading : %d \n",heading);
                }                                    
            }
            mA.speed(0);
            mB.speed(0);
        
}