File content as of revision 12:7598d38e7d44:

#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 frontdistance = 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 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.02);
    };
    
    // 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(.45);
        }
        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(.45);
        }
        stop();
        wait(0.02);
    };
    
    // Turn L or R, "mag" of turn prop. to dist. from wall
    void turnLeftScaled(double scale) {
        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;

void dist(int distance) {
    frontdistance = distance;
}
void dist2(int distance)   // left sensor interrupt
{
    leftdistanceending = distance;
}
void dist3(int distance)  // right sensor interrupt
{
    /*if (distance < 150) {
     pc.printf("Right Sensor trigg"); 
    }
    else {
        pc.printf("Right Sensor echo");
    }*/
    //rightdistance = distance;
}
//------------------|
// HELPER FUNCTIONS |
//------------------|

//------------------------------|
// PIN INITIALIZATIONS (cont'd) |
//------------------------------|
// Setup Ultrasonic Sensor pins:
ultrasonic usensor(p15, p16, .07, 1, &dist);  // trigger to p13, echo to p14...
ultrasonic usensor2(p15, p26, .07, 1, &dist2);  // left sensor trigger
ultrasonic usensor3(p15, p25, .07, 1, &dist3); //right sensor trigger
// PUT CODE HERE TO TRIGGER WHEN DISTANCE READS LESS THAN 15cm 
void checkcollision()  // NOTE: by default "distance" is in mm...
{
    if (frontdistance < 150) {
        
        mctrl.stop();

        // Turn 90 degrees
        if (tr) {
            mctrl.turnRight();
        } else {
            mctrl.turnLeft();
        }
        
        // Go forward X (1?) number of seconds.
        // change dstance???? globals??
        float currt = t.read();
        usensor.startUpdates();
        wait(.5);
        usensor.checkDistance();
        while (((t.read() - currt) < .75) && frontdistance > 150) {
            mctrl.fwd();
            usensor.startUpdates();
            wait(.5);
            usensor.checkDistance();
        }
        // Turn 90 degrees
        if (tr) {
            mctrl.turnRight();
        } else {
            mctrl.turnLeft();
        }

        tr = !tr;
    } else {
        // Continue forward until wall.
        mctrl.fwd();  
    }
}

//---------------|
// 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);
    
    // obtain and print starting distances
    int forwardDist = usensor.getCurrentDistance();
    int leftdistancestarting = usensor2.getCurrentDistance();
    int rightDist = usensor3.getCurrentDistance();
    wait(0.5);
    
    pc.printf("Current Forward Starting Distance %d mm\r\n", forwardDist);
    pc.printf("Current Left Starting Distance %d mm\r\n", leftdistancestarting);
    pc.printf("Current Right Starting Distance %d mm\r\n", rightDist);

    // Initialize the Timer:
    t.start();
    
   //while(1) {
        while (!sw) {
            usensor.checkDistance();
            checkcollision();            
            //determine which sensor is closest to wall
            int currLeftDist = usensor2.getCurrentDistance();
            int currRightDist = usensor3.getCurrentDistance();
            
            // may also have to scale the difference (40, etc.) based on - 
            // iteration of the 90deg turn sequence (loss of accuracy)
            // or decrease numbers of turns
            
            //if left sensor closest to wall
            if (currLeftDist > 100 && currRightDist > 100) {
                //mctrl.rev();
                mctrl.fwd();
            } else {
            if (currLeftDist < currRightDist) {
                //if robot has drifted toward the wall by a lot, scale R turn prop.
                if ((leftdistancestarting - currLeftDist) > 40) {
                    mctrl.turnRightScaled(1);
                    mctrl.fwd();
                //if drifted toward wall a little, scale accordingly
                } else if ((leftdistancestarting - currLeftDist) > 5) {
                    mctrl.turnRightScaled(0.75);
                    mctrl.fwd();
                //if drift away a lot, scale L turn accordingly
                } else if ((currLeftDist - leftdistancestarting) > 40) {
                    mctrl.turnLeftScaled(1);
                    mctrl.fwd();
                } else if ((currLeftDist - leftdistancestarting) > 5) {
                    mctrl.turnLeftScaled(0.75);
                    mctrl.fwd();
                } //else {
//                    mctrl.fwd();
//                }
            } else if (currLeftDist > currRightDist) {
                //if robot has drifted toward the wall by a lot, scale L turn by a lot
                if ((rightDist - currRightDist) > 40) {
                    mctrl.turnLeftScaled(1);
                    mctrl.fwd();
                //if drifted toward wall a little, scale accordingly
                } else if ((rightDist - currRightDist) > 5) {
                    mctrl.turnLeftScaled(0.75);
                    mctrl.fwd();
                //if drift away a lot, scale R turn accordingly
                } else if ((currRightDist - rightDist) > 40) {
                    mctrl.turnRightScaled(1);
                    mctrl.fwd();
                } else if ((currRightDist - rightDist) > 5) {
                    mctrl.turnRightScaled(0.75);
                    mctrl.fwd();
                } 
            }
            // may be weird when updating after turning? maybe wait/etc.
            leftdistancestarting = currLeftDist;
            rightDist = currRightDist;
            wait(.03);
            }       
        }
        mctrl.stop();
}