File content as of revision 20:e844f2e48691:

    #include "mbed.h"
    #include "m3pi_ng.h"
    #include "time.h"
    #include "btbee.h"
    #include "MSCFileSystem.h"
    #include <vector>
    
    #define FSNAME "msc"
    MSCFileSystem msc(FSNAME);
    
    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 25
    
    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 LoopTime;
        Timer WriteTime;
        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;
        double 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;
        vector < double > ZeTimes(2);
        vector < vector < double > > Data;
        double stuff;
        double junk;
        double timer;
        double TimeToWrite;
        int loopcounter = 0;
       
        
        btbee.printf("\n");
        btbee.printf("Now starting \n");
        
        while (1) {
            
            LoopTime.start();
    
            
           // Get the position of the line.
            current_pos_of_line = m3pi.line_position(); 
            
            stuff = current_pos_of_line;
                  
            proportional = current_pos_of_line;
            //fprintf(fp, "%f", current_pos_of_line);
            //fprintf(fp, "        ");
            
            // 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);
            
            
            
        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;
                    }
                }    
            }
            
            ZeTimes[0]= stuff;
            ZeTimes[1] = power;
            Data.push_back(ZeTimes);
            
            loopcounter++;
    
            
            timer = LoopTime.read();
            
            if(timer < 0.005) {
                wait(0.005 - timer);
            }
        
        timer = 0;
        LoopTime.reset();
            
        if (loopcounter == 10) {
            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);
            btbee.printf("\n");
            btbee.printf("Printing to USB");
            
            WriteTime.start();
            
            DIR *d;
            struct dirent *p;
            d = opendir("/" FSNAME);
            FILE *fp = fopen( "/" FSNAME "/Genau.txt", "w"); //file name
            if ( fp == NULL ){
                m3pi.leds(1);
                }
        
            for(int i = 0; i < 10; i++) {
                for( int j = 0; j < 2; j++) {
                    junk = Data[i][j];
                    fprintf(fp,"%f        ", junk);
                    }
                fprintf(fp,"\r\n");
                }
            m3pi.cls();
            TimeToWrite = WriteTime.read();
            btbee.printf("%f    \n", TimeToWrite);
            WriteTime.reset();
        
            btbee.printf("Printed");
            m3pi.cls();
            m3pi.printf("Printed");
        
            fclose(fp);
            
            exit(1);
        }
    
    
    
    }
        
    }
    
    }