File content as of revision 1:b7028d9ab40d:

#include "mbed.h"
#include "m3pi_ng.h"
#include "time.h"
#include "btbee.h"
#include <vector>
#include <string>

m3pi m3pi;
btbee btbee;
DigitalIn m3pi_pb(p21);
DigitalIn m3pi_IN[] = {(p12)};
DigitalOut mbed_led[] = {(LED1), (LED2),(LED3), (LED4)};
DigitalOut m3pi_led[] = {(p13), (p14), (p15), (p16), (p17), (p18), (p19), (p20)};

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

// PID terms
#define P_TERM 1.5
#define I_TERM 0
#define D_TERM 0.1

int main() {
    
    
while(1) {
    
    btbee.reset();
    m3pi_pb.mode(PullUp);
    
    m3pi.printf("Wait 4");
    m3pi.locate(0,1);
    m3pi.printf("PC");
    
    while(m3pi_pb) {
        m3pi_led[0]=!m3pi_led[0];
        wait(3);
        btbee.printf("\n");
        btbee.printf("PC connected. Press the button \n");
    }
    
    m3pi.cls();
    
    Timer LapTimer;
    Timer LoopTimer;
    
    char Bat[] = {'V','o','l','t',' ','i','s'};
  m3pi.print(Bat,7);
  wait(0.75);
  m3pi.cls();
   float batteryvoltage = m3pi.battery();
   char* str = new char[30];
   sprintf(str, "%.4g", batteryvoltage);
   m3pi.print(str,6);
   btbee.printf("Battery voltage is %f \n", batteryvoltage);
   wait(0.75);
   m3pi.cls();
   
    if (batteryvoltage < 4.2)
     {
    char low[] = {'L','o','w',' ','b','a','t'};
    m3pi.print(low,7);
    char ExitSound[]={'V','1','5','O','6','E','4','O','5','E','4'};
    m3pi.playtune(ExitSound,11);
    btbee.printf("Battery voltage is too low. Stopping program");
    exit(1);
     }

    m3pi.locate(0,1);
    m3pi.printf("Line PID");

    wait(2.0);

    btbee.printf("Now calibrating \n");

    m3pi.sensor_auto_calibrate();
    
    btbee.printf("Finished calibrating \n");
    

    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 LapTest[5];
    int s1, s2, s3, s4, s5;
    int counter = -1;
    int checkvar = 0;
    double Time1, Time2, Time3, Time4, Time5, TimeAve;
    btbee.printf("\n");
    btbee.printf("Now starting \n");
    int number = 0;
    double dt;
    vector < double > uvalues;
    vector < double > evalues;
    double u;
    double e;
    
    
    
    while (1) {
        
        if(number == 0) {
            
            dt = 0.005;
            number = 1;
            
            }
        else {
            
        LoopTimer.stop();
        dt = LoopTimer.read();
        
        }
        
        LoopTimer.reset();
        LoopTimer.start();
        
       // Get the position of the line.
        current_pos_of_line = m3pi.line_position();        
        proportional = current_pos_of_line;
        e = current_pos_of_line;
        
        // Compute the derivative
        derivative = (current_pos_of_line - previous_pos_of_line)/(dt);
        
        // Remember the last position.
        previous_pos_of_line = current_pos_of_line;
        
        // Compute the power
        power = (proportional * (P_TERM) ) + (derivative*(D_TERM)) ;
        u = power;
        
        // 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);
        
    uvalues.push_back(u);
    evalues.push_back(e);
        
    m3pi.calibrated_sensor(LapTest);
    
    s1 = LapTest[0];
    s2 = LapTest[1];
    s3 = LapTest[2];
    s4 = LapTest[3];
    s5 = LapTest[4];
    
    if (s1 > 500 and s5 > 500) {
        checkvar = 1;
        }
        
    if (s1 < 500 and s5 < 500 and checkvar == 1 and counter == -1) {
        LapTimer.start();
        }
        
    if (s1 < 500 and s5 < 500 and checkvar == 1) {
        counter = counter + 1;
        m3pi.cls();
        m3pi.printf("%d", counter);
        checkvar = 0;
        switch (counter) {
            case 1: {
                Time1 = LapTimer.read();
                LapTimer.reset();
                LapTimer.start();
                btbee.printf("Lap 1: %f \n", Time1);
                break;
                }
            case 2: {
                Time2 = LapTimer.read();
                LapTimer.reset();
                LapTimer.start();
                btbee.printf("Lap 2: %f \n", Time2);
                break;
                }
            case 3: {
                Time3 = LapTimer.read();
                LapTimer.reset();
                LapTimer.start();
                btbee.printf("Lap 3: %f \n", Time3);
                break;
                }
            case 4: {
                Time4 = LapTimer.read();
                LapTimer.reset();
                LapTimer.start();
                btbee.printf("Lap 4: %f \n", Time4);
                break;
                }
            case 5: {
                Time5 = LapTimer.read();
                LapTimer.reset();
                LapTimer.start();
                btbee.printf("Lap 5: %f \n", Time5);
                break;
                }
            }
                
        
        }
        
    if (counter == 5) {
        m3pi.stop();
        m3pi.cls();
        m3pi.locate(0,0);
        m3pi.printf("Ave time");
        TimeAve = (Time1 + Time2 + Time3 + Time4 + Time5)/5.0;
        m3pi.locate(0,1);
        m3pi.printf("%f", TimeAve);
        btbee.printf("Average time per lap: %f ", TimeAve);
        exit(1);
        }


    }
    
}

}