File content as of revision 6:d25157f50f14:

#include "mbed.h"
#include "LSM9DS1.h"    // IMU
#include "ultrasonic.h" // Ultrasonic Sensor
#include "Motor.h"      // Motor Drivers
#include "PinDetect.h"  // Pin Detect (for switch)


// Global Variables:
bool tr = true;
bool turnflag = false;

//---------------------|
// PIN INITIALIZATIONS |
//---------------------|
// Debug LED pin:
//DigitalOut led(p25);
DigitalIn sw(p20);
Serial pc(USBTX,USBRX);
// 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:
    // 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(float currt){
        stop();
        while ((t.read()-currt) < 1.4) {
            Lfront.speed(-1);
            Lback.speed(-1);
            Rfront.speed(1);
            Rback.speed(1);
        }
        stop();
        wait(0.02);
    };
    
    // Turn right 90 degrees:
    void turnRight(float currt){
        stop();
        while ((t.read()-currt) < 1.4) {
            Lfront.speed(1);
            Lback.speed(1);
            Rfront.speed(-1);
            Rback.speed(-1);
        }
        stop();
        wait(0.02);
    };
} mctrl;

//------------------|
// HELPER FUNCTIONS |
//------------------|
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 90 degrees (hardcoded => 1.4 seconds).
    // 2) Go forward 1.5 seconds.
    // 3) Turn 90 degrees (hardcoded => 1.4 seconds).
    // 4) Continue forward until wall.
        // [Step 1]
        float currt = t.read();
        if (tr) {mctrl.turnRight(currt);} else {mctrl.turnLeft(currt);}
        // [Step 2]
        currt = t.read();
        while ((t.read()-currt) < 1.5) {mctrl.fwd();}
        // [Step 3]
        currt = t.read();
        if (tr) {mctrl.turnRight(currt);} else {mctrl.turnLeft(currt);}
        // [End]
        tr = !tr;
    }
    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() {
    // Use internal pullup for the switch:
    sw.mode(PullUp);
    // Initialize the Ultrasonic Sensor:
    usensor.startUpdates();
    
    // Initialize the Timer:
    t.start();
    pc.printf("sw is : %d", sw);
    while(!sw) {
    
        usensor.checkDistance();
        
        wait(0.1);
    }
}