File content as of revision 1:f4dd7bc26785:

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

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

// PID terms
#define P_TERM 1
#define I_TERM 0
#define D_TERM 2   // with one call printf 

// Declaration 
m3pi    pi;  //pi is of the classtyp m3pi
btbee   bee; //bee is of the classtyp mtbee


//  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();
}

// function calibration //////
void calib()
{
    char complete=pi.sensor_auto_calibrate();
    // sensor_auto_calibrate, wie geht das intern?Atmel
    if(complete=='c')
    {
    pi.locate(0,0);
    pi.printf("calibrat");
    pi.locate(0,1);
    pi.printf("complete");
    wait(2);
    }else
    {
    pi.locate(0,0);
    pi.printf("calibrat");
    pi.locate(0,1);
    pi.printf("failed");
    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(float * integrator_ptr, float * prev_line_ptr)
// argument passed by address, 
// integrator_ptr is a pointer that points to an address
// in main 

{


    // 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 diff_speed;
    float speed = MAX;

    //integral = *integrator_ptr;
    //previous_pos_of_line = *prev_line_ptr;


    // 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 - *prev_line_ptr;//previous_pos_of_line;
                        // pointer points to the address where the
                        // value of the previous position x(t_{i-1}) is stored
                        
    // Compute the integral
    integral += proportional;
    *integrator_ptr = integral;
    

    // Remember the last position.
    //previous_pos_of_line = current_pos_of_line;
    *prev_line_ptr = current_pos_of_line; // x(t_i) is put to the address 
                                            // where 

    // Compute the diff_speed
    
    diff_speed = (proportional * (P_TERM) ) + (*integrator_ptr*(I_TERM)) + (derivative*(D_TERM)) ;
    // Compute new speeds
    right = speed+diff_speed;
    left  = speed-diff_speed;

    // 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()
{
// declaration //////
    
    float cross_time;
    float cross_speed; 
    float integrator;
    float prev_line_pos;
    
// initialation ////
    
    //how long does it take to move over the cross?
            // 9.5/2cm=4.25cm // half diameter of the m3pi
            // 0 < speed < 1
            // measure: speed1=0.1 s1=42cm t1=5.5s voltage1=4.4 V
            // measure: speed1=0.2 s1=42cm t1=2.1s 
            
            // real speed1=42/5.5 cm/s=7.6 cm/s
            // dt1=4.25/7.6s=0.6s
            // real speed2=42/2.1 cm/s=20 cm/s
            // dt2=4.24/20s=0.212s
    
    cross_time=0.22;
     
    integrator=0.0;
    prev_line_pos=0.0;
    cross_speed = MAX*0.6;//  MAX=0.2 60%


    pi.cls();
    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(&integrator, &prev_line_pos);// zurücksetzen von integrator und pos x(t_i)=0
        } 
        else if(outer_sens()==1 && inner_sens()==1 ) // outer black, inner black
        { 
            pi.cls();
            pi.stop();
            pi.locate(0,0);
            pi.printf("cross");
            pi.locate(0,1);
            pi.printf("stop");
            wait(0.2);
            // move just little further
            pi.left_motor(cross_speed);
            pi.right_motor(cross_speed);
            wait(cross_time);
            

            /////////////////////////////////////////////////////////////////////////
            // t-cross?
            //////////////////////////////////////////////////////////////////////////
            if(outer_sens()==0 && inner_sens()==0 )// outer white, inner white
                
            {
                pi.stop();
                pi.cls();
                pi.locate(0,0);
                pi.printf("T end");
            } else
                /////////////////////////////////////////////////////////////////////////
                // cross-cross
                if(outer_sens()==0 && inner_sens()==1 ) { // outer white, inner black
                    //turn_right();
                
                }//second else
         }//first else
    }//while true
 
}