File content as of revision 0:9e014841f2b7:

#include "mbed.h"
#include "LSM9DS1.h"    // IMU
#include "ultrasonic.h" // Ultrasonic Sensor
#include "Motor.h"      // Motor Drivers


//---------------------|
// PIN INITIALIZATIONS |
//---------------------|
// Setup IMU pins:
LSM9DS1 IMU(p28, p27, 0xD6, 0x3C);  // SDA to p28, SCL to p27..

// Setup Motor Driver pins:
Motor Lfront(p21, p6, p5);  // PWM to p21, forward to p6, reverse to p5...
Motor Lback(p22, p8, p7);  // PWM to p22, forward to p8, reverse to p7...
Motor Rfront(p23, p10, p9);  // PWM to p23, forward to p10, reverse to p9...
Motor Rback(p24, p12, p11);  // PWM to p24, forward to p12, reverse to p11...

Timer t;

//-----------------------|
// CLASS INITIALIZATIONS |
//-----------------------|
class mctrl {
public:
    void stop(void);
    void fwd(float s);
    void rev(float s);
    void turnLeft(float s);
    void turnRight(float s);
private:
    float tcurr;
    float turntime
};

//------------------|
// HELPER FUNCTIONS |
//------------------|
void mctrl::stop(void)
{
    Lfront.speed(0);
    Lback.speed(0);
    Rfront.speed(0);
    Rback.speed(0);
    wait(0.02);
}

void mctrl::fwd(float s)
{
    mctrl.stop();
    
    Lfront.speed(s);
    Lback.speed(s);
    Rfront.speed(s);
    Rback.speed(s);
    wait(0.02);
}

void mctrl::rev(float s)
{
    mctrl.stop();
    
    Lfront.speed(-s);
    Lback.speed(-s);
    Rfront.speed(-s);
    Rback.speed(-s);
    wait(0.02);
}

void mctrl::turnLeft(float s)
{
    mctrl.stop();
    
    tcurr = t.read();
    while ((t.read() - tcurr) < turntime) {
        Lfront.speed(-s);
        Lback.speed(-s);
        Rfront.speed(s);
        Rback.speed(s);
    }
    
    mctrl.stop();
    wait(0.02);
}

void mctrl::turnRight(float s)
{
    mctrl.stop();
    
    tcurr = t.read();
    while ((t.read() - tcurr) < turntime) {
        Lfront.speed(s);
        Lback.speed(s);
        Rfront.speed(-s);
        Rback.speed(-s);
    }
    
    mctrl.stop();
    wait(0.02);
}

void dist(int distance)  // NOTE: by default "distance" is in mm...
{
    if (distance < 150) {
        // PUT CODE HERE TO TRIGGER WHEN DISTANCE READS LESS THAN 15cm
        // 1) Turn right.
        // 2) Go forward __ seconds.
        // 3) Turn right.
        // 4) Continue forward until wall.
    }
}

//------------------------------|
// PIN INITIALIZATIONS (cont'd) |
//------------------------------|
// Setup Ultrasonic Sensor pins:
ultrasonic usensor(p13, p14, .07, 1, &dist);  // trigger to p13, echo to p14...
                                              // update every .07 secs w/ timeout after 1 sec...
                                              // call "dist" when the distance changes...

//---------------|
// MAIN FUNCTION |
//---------------|
int main() {
    // Initialize and calibrate the IMU:
    IMU.begin();
    if (!IMU.begin()) {
        pc.printf("Failed to communicate with LSM9DS1.\n");
    }
    IMU.calibrate();
    
    // Initialize the Ultrasonic Sensor:
    usensor.startUpdates();
    
    // Initialize the Timer:
    t.start();
    
    while(1) {
        // Read IMU gyro:
        while(!IMU.gyroAvailable());
        IMU.readGyro();
            // X - IMU.calcGyro(IMU.gx)
            // Y - IMU.calcGyro(IMU.gx)
            // Z - IMU.calcGyro(IMU.gz)
        
        // Read Ultrasonic Sensor:
        usensor.checkDistance();
        
        // ...
        
        wait(0.1);
    }
}