File content as of revision 2:73f7cc4c30c7:

#include "mbed.h"
#include "m3pimaze.h"

BusOut leds(LED1,LED2,LED3,LED4);
m3pi m3pi(p23,p9,p10);
char path[1000] = "";
unsigned char path_length = 0; // the length of the path so far
int sensors[5];
DigitalIn pb(p21);

// Minimum and maximum motor speeds
#define MAX 0.25
#define MIN 0

// PID terms
#define P_TERM 1
#define I_TERM 0
#define D_TERM 20

/*==========================================================================================================================
            FOLLOW LINE
============================================================================================================================*/
void follow_line()
{

    m3pi.locate(0,1);
    m3pi.cls();
    m3pi.printf("GO!!");

    //wait(2.0);

    //m3pi.sensor_auto_calibrate();//don't want this here.

    float right;
    float left;
    float current_pos_of_line = 0.0;
    float previous_pos_of_line = 0.0;
    float derivative,proportional,integral = 0;
    float power;
    float speed = MAX;
    int foundjunction=0;
    int countdown=100; //make sure we don't stop for a junction too soon after starting
    while (foundjunction==0) {

        // Get the position of the line.
        current_pos_of_line = m3pi.line_position();
        proportional = current_pos_of_line;

        // Compute the derivative
        derivative = current_pos_of_line - previous_pos_of_line;

        // Compute the integral
        // integral += proportional;

        // Remember the last position.
        previous_pos_of_line = current_pos_of_line;

        // Compute the power
        power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;

        // Compute new speeds
        right = speed+power;
        left  = speed-power;

        // limit checks
        if (right < MIN)
            right = MIN;
        else if (right > MAX)
            right = MAX;

        if (left < MIN)
            left = MIN;
        else if (left > MAX)
            left = MAX;

        // set speed
        m3pi.left_motor(left);
        m3pi.right_motor(right);

        if (countdown>0) countdown--;
        else {
            // Next, we are going to use the sensors to look for whether there is still a line ahead
            // and try to detect dead ends and possible left or right turns.
            m3pi.readsensor(sensors);

            if((sensors[1] < 400) && (sensors[2] < 400) && (sensors[3] < 400)) {
                // There is no line visible ahead, and we didn't see any
                // intersection.  Must be a dead end.
                foundjunction=1;
                return;
            } else if((sensors[0] > 700) || (sensors[4] > 700)) {
                // Found an intersection.
                foundjunction=1;
                return;
            } else foundjunction=0;

        }
    }
}
/*=========================================================================================================================
            TURN
===========================================================================================================================*/
// This function decides which way to turn during the learning phase of
// maze solving.  It uses the variables found_left, found_straight, and
// found_right, which indicate whether there is an exit in each of the
// three directions, applying the "left hand on the wall" strategy.

char turn(unsigned char found_left, unsigned char found_forward, unsigned char found_right)
{
    // The order of the statements in this "if" is sufficient to implement a follow left-hand wall algorithm
    if(found_left)
        return 'L';
    else if(found_forward)
        return 'F';
    else if(found_right)
        return 'R';
    else
        return 'B';
}
/*==========================================================================================================================
            DO TURN
============================================================================================================================*/

void doturn(unsigned char dir)
{
    if (dir=='L') {
        m3pi.left(0.25);
        wait(0.28);
    } else if(dir=='R') {
        m3pi.right(0.25);
        wait(0.28);
    }
    //else if(dir=='F')
    //{m3pi.forward(0.3);wait(0.15);}
    else if(dir=='B') {
        m3pi.right(0.25);
        wait(0.6);
    }

    m3pi.forward(0.1);
    wait(0.1);
    m3pi.forward(0);
    return;
}
/*==========================================================================================================================
            SIMPLIFY
============================================================================================================================*/
// change LBL to S (etc), to bypass dead ends
void simplify()
{
    // only simplify the path if the second-to-last turn was a 'B'
    if((path_length < 3) || (path[path_length-2] != 'B'))
        return;


    int total_angle = 0;
    int i;
    for(i=1; i<=3; i++) {
        switch(path[path_length-i]) {
            case 'R':
                total_angle += 90;
                break;
            case 'L':
                total_angle += 270;
                break;
            case 'B':
                total_angle += 180;
                break;
        }
    }

    // Get the angle as a number between 0 and 360 degrees.
    total_angle = total_angle % 360;

    // Replace all of those turns with a single one.
    switch(total_angle) {
        case 0:
            path[path_length - 3] = 'F';
            break;
        case 90:
            path[path_length - 3] = 'R';
            break;
        case 180:
            path[path_length - 3] = 'B';
            break;
        case 270:
            path[path_length - 3] = 'L';
            break;
    }

    // The path is now two steps shorter.
    path_length -= 2;
}
/*==========================================================================================================================
            PRESSED
============================================================================================================================*/
void pressed()
{
    
    // wait 1s to give time to GET OUT OF WAY
    wait(1);
    // 

    // Re-run the maze.  It's not necessary to identify the
    // intersections, so this loop is really simple.
    int i;
    for(i=0; i<path_length; i++) {
        follow_line();

        // Drive straight while slowing down
        //m3pi.forward(0.5);
        //wait(0.05);
        m3pi.forward(0.2);
        wait(0.2);

        // Make a turn according to the instruction stored in
        // path[i].
        doturn(path[i]);
    }

    // Follow the last segment up to the finish.
    follow_line();
    m3pi.forward(0.2);
    wait(0.6);

    //celebrate with music
    char dixie[]={'V','1','5','O','5','G','1','6','E','1','6','C','8','R','2','4','C','8','R','2','4','C','1','6','D','1','6','E','1','6','F','1','6','G','8','R','2','4','G','8','R','2','4','G','8','E','1','6'};
    int numb=49;
    m3pi.playtune(dixie,numb);
    return;

    // Now we should be at the finish!  Restart the loop.

}

/*=========================================================================================================================
        MAZESOLVE
===========================================================================================================================*/
// This function is called once, from main.c.
void mazesolve()
{

    // These variables record whether the robot has seen a line to the
    // left, straight ahead, and right, while examining the current
    // intersection.
    unsigned char found_left=0;
    unsigned char found_forward=0;
    unsigned char found_right=0;
    unsigned char found_back=0;
    int sensors[5];
    // Loop until we have solved the maze.
    while(1) {

        // Follow the line until an intersection is detected
        follow_line();

        // Bump forward a bit in case sensor was triggered at an angle
        m3pi.forward(0.1);
        wait(0.1);

        found_left=0;
        found_forward=0;
        found_right=0;

        m3pi.readsensor(sensors);/*sensors are turned off outside the follow line
         function so we must call sensors eachtime they are needed*/

        // Check for left and right exits.
        if((sensors[1]<700) && (sensors [2]<700) && (sensors[3]<700)) {
            found_back=1;
        }

        if(sensors[0] > 700) {
            // Drive straight a bit more - this is enough to line up our
            // wheels with the intersection.
            m3pi.forward(0.2);
            wait(0.2);
            m3pi.readsensor(sensors);//what's beyond the intersection
            // Check for the ending spot.
            // If all five sensors are on dark black, we have
            // solved the maze.
            if((sensors[0]>900) && (sensors[1] > 900) && (sensors[2] > 900) && (sensors[3] > 900) && (sensors[4]>900)) {
                //move upon the home pad to show off
                m3pi.forward(0.2);
                wait(0.4);
                m3pi.printf("Press P21 Restart");
                break;
            } else found_left = 1;

        }

        else if(sensors[4] > 700 ) {
            //move wheels to intersection
            m3pi.forward(0.2);
            wait(0.2);
            //what is past the intersection
            m3pi.readsensor(sensors);
            // Check for the ending spot.
            // If all five sensors are on dark black, we have
            // solved the maze.
            if((sensors[0]>900) && (sensors[1] > 900) && (sensors[2] > 900) && (sensors[3] > 900) && (sensors[4]>900)) {
                m3pi.forward(0.2);
                wait(0.4);
                m3pi.printf("Press P21 Restart");
                break;
            }
            //can we go forward
            else if((sensors[1] > 700 )|| (sensors[2] > 700) || (sensors[3] > 700)) {
                found_forward = 1;

            }
            //then go right
            else found_right=1;
        }

        //debug code
        m3pi.cls();
        if (found_left==1)
            m3pi.printf("L");
        if (found_right==1)
            m3pi.printf("R");
        if (found_forward==1)
            m3pi.printf("F");
        if (found_back==1)
            m3pi.printf("B");
        //wait (3);


        unsigned char dir = turn(found_left, found_forward, found_right);

        // Make the turn indicated by the path.
        //doturn(dir);
        doturn(dir);
        // Store the intersection in the path variable.
        path[path_length] = dir;
        path_length ++;

        // Need to insert check to make sure that the path_length does not
        // exceed the bounds of the array.

        // Simplify the learned path.
        simplify();

    }

    // Solved the maze!

    // Now enter an infinite loop - we can re-run the maze as many
    // times as we want to.
    while(1) {
        m3pi.forward(0.0);
        pb.mode(PullUp);
        wait(1);

        if(pb) {
            do {

            } while(pb);
            pressed();
        }

    }


}

/*=========================================================================================================================
                    MAIN
===========================================================================================================================*/
int main()
{
//  int sensors[5];
    m3pi.locate(0,1);
    m3pi.sensor_auto_calibrate();
    m3pi.printf("MazeSolve");

    wait(2.0);

    mazesolve();

    m3pi.forward(0.0);
}