File content as of revision 3:c07ea8bf242e:

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

// Motor Power Control pins:
/*
DigitalOut Ctrl1(p29);
DigitalOut Ctrl2(p30);
PinDetect sw(p20);
*/

// 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(void){
        stop();
        while ((t.read()-currt) < 2) {
            Lfront.speed(-1);
            Lback.speed(-1);
            Rfront.speed(1);
            Rback.speed(1);
        }
        stop();
        wait(0.02);
    };
    
    // Turn right 90 degrees:
    void turnRight(void){
        stop();
        while ((t.read()-currt) < 2) {
            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 => 2 seconds).
    // 2) Go forward 2 seconds.
    // 3) Turn 90 degrees (hardcoded => 2 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) < 2) {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);
    
    // Delay for initial pullups to take effect:
    //wait(0.01);
    
    // Initialize and calibrate the IMU:
    /*
    LSM9DS1 imu(p28, p27, 0xD6, 0x3C);  // SDA to p28, SCL to p27...
    imu.begin();
    imu.calibrate(1);
    imu.calibrateMag(0);
    
    imu.readGyro();
    start = imu.gz;
    */
    
    // Initialize the Ultrasonic Sensor:
    usensor.startUpdates();
    
    // Initialize the Timer:
    t.start();
    
    while(1) {
        // Toggle motor battery banks:
        /*
        if (!sw == 1) {
            Ctrl1 = 1;
            Ctrl2 = 1;
            wait(0.2);
        }
        else {
            Ctrl1 = 0;
            Ctrl2 = 0;
            wait(0.2);
        }
        */
        
        // Read IMU gyro:
        //while(!imu.gyroAvailable());
        //imu.readGyro();
        //heading = imu.calcGyro(imu.gz);
            // X - imu.calcGyro(imu.gx)
            // Y - imu.calcGyro(imu.gy)
            // Z - imu.calcGyro(imu.gz)
            
        //if (heading > 90) {led=1;} else {led=0;}
        
        // Read Ultrasonic Sensor:
        usensor.checkDistance();
        
        wait(0.1);
    }
}