File content as of revision 15:62648eb5df9f:

#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;
    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;
    vector < float > ZeTimes(2);
    vector < vector < float > > Data(500);
    float stuff;
    float 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[loopcounter] = ZeTimes;
        
        loopcounter++;

        
        timer = LoopTime.read();
        
        if(timer < 0.005) {
        wait(0.005 - timer);
        }
    timer = 0;
    LoopTime.reset();
        
    if (loopcounter == 500) {
        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 < 500; i++) {
        
        for( int j = 0; j < 2; j++)  {
            
            junk = Data[i][j];
            
            fprintf(fp,"%f        ", junk);
            
            }
            
        fprintf(fp,"\r\n");
        
        }
    
   TimeToWrite = WriteTime.read();
   btbee.printf("%f    \n", TimeToWrite);
   WriteTime.reset();
    
    btbee.printf("Printed");
    m3pi.cls();
    m3pi.printf("Printed");
    
    fclose(fp);
        
        
        exit(1);
        }



    }
    
}

}