File content as of revision 3:bae8eb81a9d7:

#include "mbed.h"
#include "btbee.h"
#include "m3pi_ng.h"
m3pi robot;
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(){
   
   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;

   /* for (int i = 0; i <5; ++i)
        current_pos[i] = 0.0; */
   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();
        break;}
        
    else if (m3pi_IN [0] == 0)
    {break;}
   
    else 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 == 5)
             {lap_time = timer.read();
             total_time += lap_time;
             average_time = total_time/lap;
             robot.printf("%f",average_time);
              y=0; 
              break;}
              else
              {  while( x[0]> 300 && x[4] > 300)
            {robot.calibrated_sensor(x);}
             lap_time = timer.read();
             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);
   
   
   
           
    }