File content as of revision 0:d1ca55d6ef68:

#include "mbed.h"                                                                   // Mbed Library
#include "m3pi_ng.h"                                                                // Nico's Library
#include "btbee.h"                                                                  // Bluetooth Library
                                                     
m3pi Rob;                                                                           // create m3pi Object
DigitalOut mbed_led[] = {(LED1), (LED2),(LED3), (LED4)};                            // LEDs of 3PI
DigitalOut m3pi_led[] = {(p13), (p14), (p15), (p16), (p17), (p18), (p19), (p20)};   // LEDs of M3PI

DigitalIn m3pi_pb(p21,PullUp);                                                      // DigitalIn: User Button P21
DigitalIn m3pi_fir(p12);                                                            // DigitalIn: Front IR-Senspor Pin: 12

Timer timer;  

//////////////////////              Variables and Parameters          ///////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////// 
int sen_raw[5], sen_cal[5], sen_norm[5]; 
int n_black = 0;
int lap = 0;

float e, u, vleft, vright;
float dt=0.005;
float t=0, t0=0, t_lap0=0, t_lap=0, t0_code=0, t_code=.1, t_code_max=0;
/////////////////////////////              Functions          ///////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////

/////// Function for Initilization the Robot Program ///////
////////////////////////////////////////////////////////////  
void Init()
    {
    Rob.locate(0,0); Rob.printf("batteries:");
    Rob.locate(0,1); Rob.printf("%0.2f V",Rob.battery());    
    wait(1);
    Rob.cls();
    Rob.sensor_auto_calibrate();
            
    for (int i = 0 ; i<8 ; i++)
    {   m3pi_led[i] = 1; wait(0.05); m3pi_led[i] = 0; }

    }    
////////// Function for Ending the Robot Program  //////////
////////////////////////////////////////////////////////////     
void End(float Data, int rows, int columns)
    {
    Rob.locate(0,0); Rob.printf("Process");
    Rob.locate(0,1); Rob.printf("done.");
    Rob.stop();
    }
////////// Function for Motor Voltage Saturation  //////////
////////////////////////////////////////////////////////////  
void sat(float &v_sat)
    {
    if (v_sat > 1)
        v_sat=1;
    if (v_sat < -1)
        v_sat=-1;
    }
////////// Function for Automatic Line Following ///////////
////////////////////////////////////////////////////////////     
void Linefollowing()    
    {
    const double Kp = 0.60;                                                             // [default: 0.8] 
    float vforw = Kp * 0.5;     
               
    e       = Rob.line_position();
    u       = Kp*e;  
    vleft   = vforw + u;
    vright  = vforw - u;
    sat(vleft); sat(vright);                // actuator saturation 
                                                                
    Rob.left_motor(vleft);                  // push to 3pi --> AC-Motor
    Rob.right_motor(vright);                // push to 3pi --> AC-Motor
    }
/////////// Function for Avoiding a FrontCrash /////////////
////////////////////////////////////////////////////////////    
void Anticrash()
    {
     while (!m3pi_fir)
        {
        Rob.stop();
             
        for (int i = 0 ; i<8 ; i++)
        {   m3pi_led[i] = 1; wait(0.05); m3pi_led[i] = 0; }      
                 
        }
             
    }
        
/////////// Function for lap time & lap counter ////////////
////////////////////////////////////////////////////////////     
void Counter()
    {
        Rob.calibrated_sensor(sen_cal);
        
        for (int i = 0 ; i<5 ; i++)
            {sen_norm[i] = sen_cal[i]/1000;}
        
        if ( (sen_norm[0]>=.8) && (sen_norm[2]>=.8) && (sen_norm[4]>=.8)  )
        {    
            n_black++;
            t = timer.read();

            if ( (n_black == 5)&&((t-t_lap0)>2) )
            {
                lap++;
                t_lap = t-t_lap0;
                t_lap0 = t;
            
                Rob.cls();
                Rob.locate(0,0);
                Rob.printf("lap: %i",lap);
                Rob.locate(0,1);   
                Rob.printf("t: %4.2f",t_lap);
            
            }      
            
        }
        else 
        {    
        n_black=0;      
        
         }    
    }
    
///////////////////////////////              MAIN               /////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////
int main() 
{
//////////////           Initialization      ///////////////
//////////////////////////////////////////////////////////// 
Init();
////////////////////////////////////////////////////////////

float tcmp = 1; 

timer.start();
    while (true)
        {
//        Anticrash();    
        t       = timer.read();
        
        if ( (t-t0) >= dt)
            {
            t0      = t;          
            t0_code = t;
            
             
            Linefollowing();
            Counter();  
                                   
            t_code  = (timer.read()-t0_code)*1000;     
            tcmp    = t_code/t_code_max;
            
            if ( tcmp> 1.5)
                {t_code_max = t_code;                           
            Rob.cls(); Rob.locate(0,1); Rob.printf("%3.2f ms",t_code_max);
                }
            }
        } 
        
/////////////////////////////////////////////////////////////////////////////////////////////////
}