File content as of revision 1:92f6242c0196:

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


// Global Variables:
float start;
float heading;
bool turnflag = false;

//---------------------|
// 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 Rfront(p22, p8, p7);  // PWM to p22, forward to p8, reverse to p7...
Motor Lback(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:
    int LeftCount;
    int RightCount;
    
    // Stop all motors:
    void stop(void) {
        Lfront.speed(0);
        Lback.speed(0);
        Rfront.speed(0);
        Rback.speed(0);
        wait(0.02);
    };
    // Go forward at constant speed:
    void fwd(void){
        stop();
        
        Lfront.speed(1);
        Lback.speed(1);
        Rfront.speed(1);
        Rback.speed(1);
        wait(0.02);
    };
    // Reverse at constant speed:
    void rev(void){
        stop();
    
        Lfront.speed(-1);
        Lback.speed(-1);
        Rfront.speed(-1);
        Rback.speed(-1);
        wait(0.02);
    };
    // Turn left 90 degrees:
    void turnLeft(void){
        stop();
    
        while ((heading-start) < 90) {
            Lfront.speed(-1);
            Lback.speed(-1);
            Rfront.speed(1);
            Rback.speed(1);
        }
        
        LeftCount++;
    
        stop();
        wait(0.02);
    };
    // Turn right 90 degrees:
    void turnRight(void){
        stop();
    
        while ((heading-start) < 90) {
            Lfront.speed(1);
            Lback.speed(1);
            Rfront.speed(-1);
            Rback.speed(-1);
        }
        
        RightCount++;
    
        stop();
        wait(0.02);
    };
} mctrl;

//------------------|
// HELPER FUNCTIONS |
//------------------|
void dist(int distance)  // NOTE: by default "distance" is in mm...
{
    if (distance < 150 || turnflag == true) {
        turnflag = true;
        
        mctrl.stop();
        // 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.
        mctrl.turnRight();
        float currt = t.read();
        while ((t.read()-currt) < 3) {
            mctrl.fwd();
        }
        mctrl.turnRight();
        if (mctrl.RightCount == 2) {
             turnflag = false;
             mctrl.RightCount = 0;
        }
    }
    else {
        mctrl.fwd();
    }
}

//------------------------------|
// 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()) {
        // Do something if it doesn't detect the IMU...
    //}
    //IMU.calibrate();
    
    //IMU.readGyro();
    //start = IMU.calcGyro(IMU.gx);
    
    // Initialize the Ultrasonic Sensor:
    usensor.startUpdates();
    
    // Initialize the Motor turn counts:
    mctrl.LeftCount = 0;
    mctrl.RightCount = 0;
    
    // Initialize the Timer:
    t.start();
    
    while(1) {
        // Read IMU gyro:
        //while(!IMU.gyroAvailable());
        //IMU.readGyro();
        //heading = IMU.calcGyro(IMU.gx);
            // X - IMU.calcGyro(IMU.gx)
            // Y - IMU.calcGyro(IMU.gy)
            // Z - IMU.calcGyro(IMU.gz)
        
        // Read Ultrasonic Sensor:
        usensor.checkDistance();
        
        wait(0.1);
    }
}