File content as of revision 13:dd2ac39ba5ba:

#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;
 
int leftDist = 0;
int rightDist = 0;
int forwardDist = 0; 
int leftForwardDist = 0;
int rightForwardDist = 0;

//---------------------|
// 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 p5, reverse to p6...
Motor Rfront(p22, p8, p7);  // PWM to p22, forward to p7, reverse to p8...
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...
LSM9DS1 imu(p28, p27, 0xD6, 0x3C);

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.55);
    };
    
    // Go forward at constant speed:
    void fwd(void){
        //stop();
        
        Lfront.speed(0.85);
        Lback.speed(0.85);
        Rfront.speed(0.85);
        Rback.speed(0.85);
        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.25);
    };
    
    // Turn left 90 degrees:
    void turnLeft(){
        stop();
        float degree = 0.0, angularV = 0.0;
        float currt = t.read();
        while(degree < 90) {
            while(imu.gyroAvailable());
            imu.readGyro();
            angularV = imu.gz;
            Lfront.speed(1);
            Lback.speed(1);
            Rfront.speed(-1);
            Rback.speed(-1);
            if(angularV > 50.0 || angularV <-50.0) 
            {
                degree += (abs(angularV))/100.00;
            }
            wait(.4);
        }
        stop();
        wait(0.02);
    };
    
    // Turn right 90 degrees:
    void turnRight(){
        stop();
        float degree = 0.0, angularV = 0.0;
        float currt = t.read();
        while(degree < 90) {
            while(imu.gyroAvailable());
            imu.readGyro();
            angularV = imu.gz;
            Lfront.speed(1);
            Lback.speed(1);
            Rfront.speed(-1);
            Rback.speed(-1);
            if(angularV > 50.0 || angularV <-50.0) {
                degree += (abs(angularV))/100.00;
            }
            wait(.4);
        }
        stop();
        wait(0.02);
    };
    
    // Turn L or R, "mag" of turn prop. to dist. from wall
    // motor speed values are normalized to -1 to 1 (aka no 1.25)
    void turnLeftScaled(double scale) {
        // try commenting out this stop otherwise robot will stop before correcting
        stop();
        Lfront.speed(-1 * scale);
        Lback.speed(-1 * scale);
        Rfront.speed(1 * scale);
        Rback.speed(1 * scale);
        //stop();
        wait(0.02);
    };
        
    void turnRightScaled(double scale) {
        stop();
        Lfront.speed(1 * scale);
        Lback.speed(1 * scale);
        Rfront.speed(-1 * scale);
        Rback.speed(-1 * scale);
        //stop();
        wait(0.02);
    };
} mctrl;

//------------------|
// HELPER FUNCTIONS |
//------------------|
// PUT CODE HERE TO TRIGGER WHEN DISTANCE READS LESS THAN 15cm 
void dist(int distance)  // NOTE: by default "distance" is in mm...
{
    if (distance < 200) {
        
        mctrl.stop();
        
        if ( leftDist < rightDist)
        {
            mctrl.turnRight();
            mctrl.stop();
            //mctrl.fwd(); 
            
        }
        
        else 
        {
            mctrl.turnLeft(); 
            mctrl.stop();
            //mctrl.fwd();
        }
    }
    
    else {
        mctrl.fwd();
    }
};

void dist2(int distance)   // left sensor interrupt
{
    if (distance < 150)
    {
        mctrl.stop();
        mctrl.turnRight();
        mctrl.stop();
    }
    
    else
    {

        mctrl.fwd(); 
    }

}

void dist3(int distance)  // right sensor interrupt
{
    if (distance < 150)
    {
        mctrl.stop();
        mctrl.turnLeft();
        mctrl.stop();
    }
    
    else
    {
        mctrl.fwd(); 
    }

}

void dist4(int distance) {
    if (distance < 130) {
        mctrl.stop();
        mctrl.turnRight();
        mctrl.stop();
    } 
    
    else {
        mctrl.fwd();
    }
}

void dist5(int distance) {
    if (distance < 130) {
        mctrl.stop();
        mctrl.turnLeft();
        mctrl.stop();
    } else {
        
        mctrl.fwd();
    }
}

//------------------------------|
// PIN INITIALIZATIONS (cont'd) |
//------------------------------|
// Setup Ultrasonic Sensor pins:
ultrasonic usensor(p15, p16, .07, 1, &dist);  // trigger to p13, echo to p14...
                                             // update every .07 secs w/ timeout after 1 sec...
                                              // call "dist" when the distance changes...
ultrasonic usensor2(p15, p26, .07, 1, &dist2);  // left sensor trigger
ultrasonic usensor3(p15, p25, .07, 1, &dist3); //right sensor trigger

// new sonars added
ultrasonic usensor4(p15, p29, .07, 1, &dist4); //sonar to the left of the forward sensor
ultrasonic usensor5(p15, p30, .07, 1, &dist5); //sonar to the right of the forward sensor


//---------------|
// MAIN FUNCTION |
//---------------|
int main() {
    // Use internal pullup for the switch:
    sw.mode(PullUp);
    
    // Initialize the Ultrasonic Sensor:
    usensor.startUpdates();
    usensor2.startUpdates();
    usensor3.startUpdates(); 
    usensor4.startUpdates();
    usensor5.startUpdates();
    wait(0.5);
    
    // obtain and print starting distances
    forwardDist = usensor.getCurrentDistance();
    leftDist = usensor2.getCurrentDistance();
    rightDist = usensor3.getCurrentDistance();
    leftForwardDist = usensor4.getCurrentDistance();
    rightForwardDist = usensor5.getCurrentDistance();
    wait(0.5);

    // Initialize the Timer:
    t.start();
    int starttime = t.read();
    
   while(1) {
        while (!sw && t.read() - starttime < 60) {
            
            
            
             usensor.checkDistance(); 
             usensor2.checkDistance();
             usensor3.checkDistance();
             usensor4.checkDistance();
             usensor5.checkDistance();
             forwardDist = usensor.getCurrentDistance();
             leftDist = usensor2.getCurrentDistance(); 
             rightDist = usensor3.getCurrentDistance();
             leftForwardDist = usensor4.getCurrentDistance();
             rightForwardDist = usensor5.getCurrentDistance(); 
             
             if ( leftForwardDist > 100 && rightForwardDist > 100 ) 
             {
             mctrl.fwd(); 
             }
             
             else{
                 
                 mctrl.rev();
                 mctrl.turnRight();
                 mctrl.turnRight();
                 mctrl.fwd();
                 
            }
             //rightForwardDist = usensor5.getCurrentDistance(); 
             /*
             pc.printf("fowardDist: %d\n ", usensor.getCurrentDistance());
             wait (1);
             pc.printf("LeftDist: %d\n", usensor2.getCurrentDistance());
             wait(1);
             pc.printf("RightDist: %d\n", usensor3.getCurrentDistance());
             wait(1);
             pc.printf("leftForwardDist: %d\n ", usensor4.getCurrentDistance());
             wait (1);
             pc.printf("rightForwardDist: %d\n", usensor5.getCurrentDistance());
             wait(1);*/

             
    
             
        }
        // if switch flipped, stop robot
        mctrl.stop();
    }

}