File content as of revision 0:0280b02789e8:

#include "mbed.h"
#include "m3pi.h"

m3pi m3pi;

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

// PID terms
#define P_TERM 0.6
#define I_TERM 0
#define D_TERM 20
DigitalIn sw(p21);
char path[100] = "";
unsigned char path_length = 0;

void follow_segment(void);
void read_line(unsigned int sensors[5]) {
    unsigned char x[10];
    int i;
    m3pi.putc(0x86);
    for (i=0; i<10; i++) {
        x[i]=m3pi.getc();
    }
    sensors[0]=x[1]*0x100+x[0];
    sensors[1]=x[3]*0x100+x[2];
    sensors[2]=x[5]*0x100+x[4];
    sensors[3]=x[7]*0x100+x[6];
    sensors[4]=x[9]*0x100+x[8];
}

char select_turn(unsigned char found_left, unsigned char found_straight, unsigned char found_right) {
    // Make a decision about how to turn.  The following code
    // implements a left-hand-on-the-wall strategy, where we always
    // turn as far to the left as possible.
    if (found_left)
        return 'L';
    else if (found_straight)
        return 'S';
    else if (found_right)
        return 'R';
    else
        return 'B';
}
void simplify_path() {
    // 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] = 'S';
            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;
}

void turn(char dir) {
    switch (dir) {
        case 'L':
            // Turn left.
            m3pi.left(0.2);
            wait(0.3125);
            break;
        case 'R':
            // Turn right.
            m3pi.right(0.2);
            wait(0.3125);
            break;
        case 'B':
            // Turn around.
            m3pi.left(0.2);
            wait(0.625);
            break;
        case 'S':
            // Don't do anything!
            break;
    }
}


int main() {
    unsigned int sensors[5];

    sw.mode(PullUp);
    m3pi.locate(0,1);
    m3pi.printf("LineMaze");
    m3pi.leds(0);
    wait(2.0);
    unsigned char ll=0;

    m3pi.sensor_auto_calibrate();
    while (1) {
        follow_segment();
        m3pi.left_motor(0.2);
        m3pi.right_motor(0.2);
        wait(0.02);
        //m3pi.left_motor(0);m3pi.right_motor(0);
        unsigned char found_left=0;
        unsigned char found_straight=0;
        unsigned char found_right=0;
        ll=0;
        read_line(sensors);
        if (sensors[0] > 1000) {
            found_left = 1;
            ll=ll|0x04;
        }
        if (sensors[4] > 1000) {
            found_right = 1;
            ll=ll|0x01;
        }
        wait(0.186);
        read_line(sensors);
        if (sensors[1] > 1000 || sensors[2] > 1000 || sensors[3] > 1000) {
            found_straight = 1;
            ll=ll|0x02;
        }
        if (sensors[1] > 1000 && sensors[2] > 1000 && sensors[3] > 1000) {
            ll=ll|0x08;
            break;
//        return;
        }
        unsigned char dir = select_turn(found_left, found_straight, found_right);
        turn(dir);
        path[path_length] = dir;
        path_length ++;

// You should check to make sure that the path_length does not
// exceed the bounds of the array.  We'll ignore that in this
// example.

// Simplify the learned path.
        simplify_path();
        m3pi.leds(ll);
    }
    m3pi.stop();

    while (1) {
        if (sw==0)break;
    }
    wait(1);
    //2nd loop
    int i;
    for (i=0; i<path_length; i++) {
        // SECOND MAIN LOOP BODY
        follow_segment();
// Drive straight while slowing down, as before.
        m3pi.left_motor(0.2);
        m3pi.right_motor(0.2);
        wait(0.02);
        read_line(sensors);
        wait(0.186);


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

    }

    // Follow the last segment up to the finish.
    follow_segment();

    m3pi.stop();


}
void follow_segment() {
    unsigned int sensors[5];
    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;

    while (1) {

        // 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);
        //Read Sensor
        read_line(sensors);
        if (sensors[1] < 650 && sensors[2] < 650 && sensors[3] < 650) {
            // There is no line visible ahead, and we didn't see any
            // intersection.  Must be a dead end.
            m3pi.leds(0x07);
            return;
        } else if (sensors[0] > 1000 || sensors[4] > 1000) {
            // Found an intersection.
            m3pi.leds(0x0f);
            return;
        }
    }
}