File content as of revision 0:1e2ff5ae5204:

#include "mbed.h"
#include "m3pi_ng.h"


// Minimum and maximum motor speeds
#define MAX 0.1
#define MIN 0

// PID terms
#define P_TERM 1
#define I_TERM 0
#define D_TERM 20





// Declaration pi is of the classtyp m3pi
m3pi pi;

//  shows the battery voltage in first line for a little? time , how long takes one for
    void battery_voltage()
    {
       float voltage;
       // 
       for(int i=0;i<100;i++) 
        {
        voltage=pi.battery();
        pi.locate(0,0);
        pi.printf("%f",voltage);
        pi.locate(0,1);
        pi.printf("%i",i);
        }
        pi.cls();
     } 
 
// calibration with c for complete on screen
    void calib()
    {
        char complete=pi.sensor_auto_calibrate();   //wozu denn?
        pi.locate(0,0);
        pi.printf("%c",complete);
        wait(2);
        pi.cls();
    }
    
    
    
    
    
    
// function gives back 1 if outer sensors on black surface //////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////
//parameter "black_or_white" is a pointer, argument has to be a address

int outer_sens()//int* outer_b_or_w)
    {
    int outer_b_or_w;
    // read raw sensor values  
    //        
    int raw_sens_values[5];
    pi.raw_sensor(raw_sens_values);
    
    // give the raw sensorvalue in 1. line                                                       
    /*    
    pi.locate(0,0);
    ///////////////////////////////////////
    wait(0.2);//value meassurement with delay (not good but good for representation of the values on scream    
    ///////////////////////////////////////
    
    pi.cls(); // ohne clear display zeigt er Werte über 2000 an
    pi.printf("%d",raw_sens_values[0]);    
    // array entspricht pointer
    */
    
    if (raw_sens_values[0]> 1000 & raw_sens_values[4]> 1000)// solange
    //auf saal:white = ca. 400; black = ca. 2000;
        {
        //pi.locate(0,1);
        //pi.printf("%i",1);
         outer_b_or_w=1;          //1 is put to the address where the
         } //black              // pointer is pointing at    
        else 
        {
         //  pi.locate(0,1);
         //  pi.printf("%i",0);
          outer_b_or_w=0;  
                               
        } //white
        
      // return by ??? 
      // return by value is slow far structures and arrays !!!!!!!!!!!!!!!!!!???????????????????
      return outer_b_or_w;
    }

// function gives back 1 if inner sensors on black surface //////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////
//parameter "black_or_white" is a pointer, argument has to be a address

int inner_sens()
    {
    int inner_b_or_w;        
    int raw_sens_values[5];
    pi.raw_sensor(raw_sens_values);
    
    
    if (raw_sens_values[2]> 1000)// solange
    //white = ca. 400; black = ca. 2000;
        {
         inner_b_or_w=1;          //1 is put to the address where the
         } //black              // pointer is pointing at    
        else 
        {
          inner_b_or_w=0;  
                               
        } 
      return inner_b_or_w;
    }


    
    
/////////////////////////////////////////////////////////////////////////////////// 
// function line_follow ///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////

//void line_follow(const int* ON)// runs only if ON points to a integer 1
  void line_follow()
// argument passed by address, ON is a pointer that points to an address 
{


    // Defines on top



    
    pi.locate(0,1);
    pi.printf("Line PID");
    
    //wait(2.0);
    

    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;
    

        // Get the position of the line.
        current_pos_of_line = pi.line_position();        
        proportional = current_pos_of_line;
        
        // 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 
        pi.left_motor(left);
        pi.right_motor(right);

    } 

/////////////////////////////////////////////////////////////////////////////////// 
// function turn right ///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
void turn_right()
    {
    float turn_speed = MAX;
     pi.left_motor(-turn_speed);// look on the wheel, pos speed => wheel turns clockwise
     pi.right_motor(turn_speed);
     wait (0.9);
    }  
///////////////////////////////////////////////////////////////////////////////////        
// main        
///////////////////////////////////////////////////////////////////////////////////



main() 
{
    
    float cross_speed = MAX*0.9;
    
    
    battery_voltage();
    calib();
      
    while(true)
    {         
        
      if(outer_sens()==0 && inner_sens()==1)//outer white, inner black
      {
        pi.locate(0,0);
        pi.printf("while %i",outer_sens());
        pi.cls();
        line_follow();
       }
       else if(outer_sens()==1 && inner_sens()==1 )// outer black, inner black 
       {   
        pi.cls();
        pi.stop();   
        pi.locate(0,0);
        pi.printf("stop");
        wait(0.2);         
        // move just little further
        pi.left_motor(cross_speed);
        pi.right_motor(cross_speed);
        wait(0.5); 
            //die zeit die erbraucht für den halben durch messer
            // 9.5/2cm=4.25cm
            // 0 < speed < 1
            // measure: speed=0.1 s=42cm t=5.5s voltage=4.4 V
            // real speed=42/5.5 cm/s=7.6 cm/s
            // dt=4.25/7.6=0.6
                        
            /////////////////////////////////////////////////////////////////////////
            // t-cross?
            if(outer_sens()==0 && inner_sens()==0 )// outer white, inner white 
            //////////////////////////////////////////////////////////////////////////
            {
            pi.cls();
            pi.stop();   
            pi.locate(0,0);
            pi.printf("T end");
            }       
            else 
            /////////////////////////////////////////////////////////////////////////
            // cross-cross
            if(outer_sens()==0 && inner_sens()==1 )// outer white, inner black 
            {//turn_right();
            }
        }              
    }
}