File content as of revision 0:e60be1842101:

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

BusOut leds(LED1,LED2,LED3,LED4);
m3pi m3pi(p23,p9,p10);

#define MAX 1.0
#define MIN 0.2


#define P_TERM 0.8
#define I_TERM 0
#define D_TERM 20

int main() {

    m3pi.locate(0,1);
    for (int countdown=3; countdown >=1; countdown--) {
        m3pi.printf("%d", countdown);
        wait(1);
        m3pi.cls();

    }
    m3pi.cls();
    wait(0.001);
    m3pi.printf("GO!");
    wait(1);
    m3pi.cls();
    m3pi.printf("VB: %f", m3pi.battery());


    float right;
    float left;
    float position_of_line = 0.0;
    float act_position_of_line = 0.0;
    float prev_pos_of_line = 0.0;
    float derivative,proportional;
    float integral = 0;
    float power;
    m3pi.sensor_auto_calibrate();
    float speed = MAX;

    while (1) {
        for (int i=0; i<1000; i++) {

            // Get the position of the line.
            act_position_of_line = m3pi.line_position();
            position_of_line = act_position_of_line - 0.40;

            proportional = position_of_line;
            // Compute the derivative
            derivative = position_of_line - prev_pos_of_line;
            // Compute the integral
            integral += proportional;
            // Remember the last position.
            prev_pos_of_line = position_of_line;
            // Compute
            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;

            if ((position_of_line < 0.20) && (position_of_line > -0.20)) {
                m3pi.left_motor(MAX);
                m3pi.right_motor(MAX);

            }

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

            //m3pi.cls();
            //m3pi.printf("VB: %f", m3pi.battery());
        }


    }
}