File content as of revision 7:7d491b51665e:

#include "mbed.h"
#include "btbee.h"
#include "m3pi_ng.h"
m3pi robot;
btbee btbee;
DigitalIn m3pi_IN[]= {(p12),(p21)}; // IR sensor and Knopf
Timer timer;
Timer time_wait;
#define MAX 0.95
#define MIN 0

//#define P_TERM 5
//#define I_TERM 0
//#define D_TERM 20



int main()
{
    float P_TERM = 1;
    float I_TERM = 0;
    float D_TERM = 20;

    btbee.reset();
    robot.sensor_auto_calibrate();
    wait(2.0);
    float right;
    float left;
    //float current_pos[5];
    float current_pos = 0.0;
    float previous_pos =0.0;
    float derivative, proportional, integral = 0;
    float power;
    float speed = MAX;

    int lap = 0;
    float lap_time = 0.0;
    float total_time = 0.0;
    float average_time = 0.0;
    int y =1;
    int count = 0;
    float paramChange[3];

    char arr_read[30]; // this should be long enough to store any reply coming in over bt.
    int  chars_read;

    /* for (int i = 0; i <5; ++i)
         current_pos[i] = 0.0; */
    
    
    wait(8);
    btbee.printf("Battery: %f\n", robot.battery());
    //timer.start();


    time_wait.start();
    while(y) {
        time_wait.reset();
        //Get raw sensor values
        int x [5];
        robot.calibrated_sensor(x);



        //Check to make sure battery isn't low
        if (robot.battery() < 2.4) {
            timer.stop();
            btbee.printf("Battery too low\n");
            break;
        }

        //else if (m3pi_IN [0] == 0)
        //{break;}

        if( x[0] > 300 && x[2]>300 && x[4]>300) {
            if (lap == 0) {
                while( x[0]> 300 && x[4] > 300) {
                    robot.calibrated_sensor(x);
                }
                timer.start();
                lap= lap +1;
            }

            else if (lap == 2) {
                robot.stop();
                lap_time = timer.read();
                total_time += lap_time;
                average_time = total_time/lap;
                robot.printf("%f",average_time);
                if (btbee.writeable()) {
                    btbee.printf("Lap %d time: %f\n", lap, lap_time);
                    btbee.printf("Avg Lap time: %f\n", average_time);
                }
                
                while (count < 3){
                    //btbee.printf("Input parameter\n");
                    btbee.read_line(arr_read, 30, &chars_read);
                    paramChange[count] = atof(arr_read);
                    //btbee.printf("%d", arr_read);
                    count++;
                }
                P_TERM = paramChange[0];
                I_TERM = paramChange[1];
                D_TERM = paramChange[2];
                btbee.printf("PTERM %f\n", P_TERM);
                btbee.printf("ITERM %f\n", I_TERM);
                btbee.printf("DTERM %f\n", D_TERM);
                lap = 0;
                total_time = 0;
                count = 0;
                timer.stop();
                timer.reset();
                continue;
                
            } else {
                while( x[0]> 300 && x[4] > 300) {
                    robot.calibrated_sensor(x);
                }
                lap_time = timer.read();
                if (btbee.writeable()) {
                    btbee.printf("Lap %d time: %f\n", lap, lap_time);
                }
                total_time += lap_time;
                average_time = total_time/lap;
                lap = lap +1;
                timer.reset();
            }
        }


        // Get the position of the line.
        /*  for (int i =0; i < 4; ++i)
             current_pos[i] = current_pos[i+1];
           current_pos[4] = robot.line_position();
        proportional = current_pos[4];

        // compute the derivative
        derivative = 0;
        for (int i =1; i<5;++i) {
             if (i ==1)
                 derivative += 0*(current_pos[i] - current_pos[i-1]);
              else if (i == 2)
                 derivative += 0*(current_pos[i] - current_pos[i-1]);
              else if (i==3)
                 derivative += 0*(current_pos[i] - current_pos[i-1]);
             else
                 derivative += (current_pos[i] - current_pos[i-1]);
         }

        derivative = derivative; */


        current_pos = robot.line_position();
        proportional = current_pos;

        derivative = current_pos - previous_pos;


        //compute the integral
        integral =+ proportional;

        //remember the last position.
        previous_pos = current_pos;

        // compute the power
        power = (proportional*(P_TERM)) + (integral*(I_TERM)) + (derivative*(D_TERM));
        //computer 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>MIN)
            left = MAX;

        //set speed

        robot.left_motor(left);
        robot.right_motor(right);

        wait((5-time_wait.read_ms())/1000);
    }



    robot.stop();

    char hail[]= {'V','1','5','O','4','E','4','C','8','D','8','E','8','C','8','D','8'
                  ,'E','8','F','4','D','8','E','8','F','8','D','8','E','8','F','8','G'
                  ,'4','A','4','E','1','6','E','1','6','F','8','C','8','D','8','E','8'
                  ,'G','8','E','8','D','8','C','4'
                 };
    int numb = 59;

    robot.playtune(hail,numb);




}