File content as of revision 10:2a1c8ce9d76c:

#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 leftdistancestarting= 0;
int leftdistanceending= 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 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...
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(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.53) {
            Lfront.speed(-1);
            Lback.speed(-1);
            Rfront.speed(1);
            Rback.speed(1);
        }
        stop();
        //wait(0.02);
    };
    
    // Turn right 90 degrees:
    void turnRight(){
        stop();
        float degree = 0.0, angularV = 0.0;
        float currt = t.read();
        /*while ((t.read()-currt) < 1.56) {
            Lfront.speed(1);
            Lback.speed(1);
            Rfront.speed(-1);
            Rback.speed(-1);
        }*/
        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);
    };
    
    
            
        
} mctrl;

//------------------|
// HELPER FUNCTIONS |
//------------------|
void dist(int distance)  // NOTE: by default "distance" is in mm...
{
    if (distance < 150) {
        
        mctrl.stop();
    // 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();} else {mctrl.turnLeft(currt);}
        // [Step 2]
        currt = t.read();
        while ((t.read()-currt) < 1) {mctrl.fwd();}
        // [Step 3]
        currt = t.read();
        if (tr) {mctrl.turnRight();} else {mctrl.turnLeft(currt);}
        // [End]
        tr = !tr;
    }
    else {
        mctrl.fwd();
        
    }
}

void dist2(int distance)   // left sensor interrupt
{
    
     leftdistanceending = distance; 
    //pc.printf(" Distance %d mm\r\n", distance);


}

void dist3(int distance)  // right sensor interrupt
{
    /*if (distance < 150) {
     pc.printf("Right Sensor trigg"); 
    }
    else {
        pc.printf("Right Sensor echo");
    }*/
    //rightdistance = distance;
}


//------------------------------|
// 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


//---------------|
// MAIN FUNCTION |
//---------------|
int main() {
    // Use internal pullup for the switch:
    sw.mode(PullUp);
    // Initialize the Ultrasonic Sensor:
    usensor.startUpdates();
    usensor2.startUpdates();
    usensor3.startUpdates(); 
    wait(0.5);
    int leftdistancestarting = usensor2.getCurrentDistance();
    wait(0.5);
    pc.printf("Current Starting Distance %d mm\r\n", leftdistancestarting);

    // Initialize the Timer:
    t.start();
    
   while(1) {
        while (!sw)
        {
    
        usensor.checkDistance();
        usensor2.checkDistance();
        usensor3.checkDistance();
        pc.printf("Current Ending Distance %d mm\r\n", leftdistanceending);

        }
        mctrl.stop();

    }
}