File content as of revision 10:fd4ae3946c7e:

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

using namespace std;

//For USB writing
#include "MSCFileSystem.h"
#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
double P_TERM = 1.5;
double I_TERM = 0;
double D_TERM = 25;
char arr_read[30];
int  chars_read;

int main() {
    
    
    
    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;
    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;
    bool IR = 1;
    double timer;
    btbee.printf("\n");
    btbee.printf("Now starting \n");
    string message;
    DIR *d;
    struct dirent *p;
   
    d = opendir("/" FSNAME);
    
    FILE *fp = fopen( "/" FSNAME "/test results.txt", "w"); //file name
    if ( fp == NULL )
    {
        m3pi.leds(1);
    }
    
    
    while (1) {
        
    LoopTime.start();
        
    IR = m3pi_IN [0];
        
    if(m3pi_IN [0] == 0 and IR == 0) {
        while (m3pi_IN [0] == 0 and IR == 0) {
    m3pi.cls();
    btbee.printf("Obstacle detected. Ending program");
    m3pi.printf("ABORT");
    m3pi.stop();
    exit(1);
    }
}

        
       // Get the position of the line.
        current_pos_of_line = m3pi.line_position();        
        proportional = current_pos_of_line;
        
        std::ostringstream ss;
        ss << current_pos_of_line;
        message = ss.str();
        message.append("     ");
        
        // 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);
        
        
        ss << left;
        message.append(ss.str());
        message.append("     ");
        ss << right;
        message.append(ss.str());
             
        
    m3pi.calibrated_sensor(LapTest);
    
    s1 = LapTest[0];
    s5 = LapTest[4];
    
    if (s1 > 500 and s5 > 500) {
        m3pi.stop();
        exit(1);
        }
       
//    if (s1 < 500 and s5 < 500 and checkvar == 1 and counter == -1) {
//        LapTimer.reset();
//        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 \n", TimeAve);
//        btbee.printf("\n");
//        btbee.printf("Enter P value, I value, and D value separately \n");
//                
//                while(1) {
//                    
//                        
//                        if (btbee.readable()) {
//                            
//                          btbee.printf("Enter P valure: \n");
//            
//            
//                          btbee.read_all(arr_read, 30, &chars_read);
//                           P_TERM = atof(arr_read);
//            
//                          btbee.printf("P term is %f \n", P_TERM);
//            
//                          wait(1.0);
//            
//            break;
//            
//                                             }
//            
//                          }
//            
//            while(1) {
//                            
//            if(btbee.readable()) {
//                
//                btbee.printf("Enter I value: \n");
//            
//                btbee.read_all(arr_read, 30, &chars_read);
//                I_TERM = atof(arr_read);
//            
//                btbee.printf("I term is %f \n", I_TERM);
//            
//                 wait(1.0);
//            
//                 break;
//            
//                                    }
//                                    
//                        }
//                        
//            while(1)  {
//                
//                if(btbee.readable()) {
//                    
//                    btbee.printf("Enter D value: \n");
//            
//                    btbee.read_all(arr_read, 30, &chars_read);
//                    D_TERM = atof(arr_read);
//            
//                    btbee.printf("D term is %f \n", D_TERM);
//                    
//                    wait(1);
//                    
//                    btbee.printf("Starting again with new parameters");
//            
//                    counter = -1;
//            
//            
//                     break;
//            
//                                    }
//            
//                      }
//              
//        }
                        
                                        
        

    //if(timer < 0.005) {
//        wait(0.005 - timer);
//        }
//    timer = 0;
//    LoopTime.reset();

        printf("%s",message);
        fprintf(fp,"\n");
        message.clear();
        
    }


    fclose(fp);
}