File content as of revision 15:fd6886ab896a:

#include "mbed.h"
#include "m3pi.h"

m3pi m3pi;

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

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

void sensor_all(int arr[]) //lav en funktion? ved ikke hvad jeg skal her
     {
        m3pi.putc(0x87); // send noget sensor data
    
        for(int n=0; n < 5; n++) // forloop så sensor for data
        {
        char lowbyte = m3pi.getc();
        char hibyte  = m3pi.getc();
        arr[n] = ((lowbyte + (hibyte << 8))); // sensor value
        }
        m3pi.cls();
        m3pi.locate(0,1);
        m3pi.printf("%d", arr[0]);
        m3pi.locate(0,0);
        m3pi.printf("%d", arr[4]);
        }
 
int maze (void)
{
    
    return 1;
}   

void turn_left (void)
{
    m3pi.left(0.3);
    wait (0.22);
}
    
void turn_right (void)
{
    m3pi.right (0.3);
    wait (0.22);
}

int main() {

    m3pi.locate(0,1);
    m3pi.printf("Line PID");

    wait(1.0);

    m3pi.sensor_auto_calibrate();

    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;
    int sensor_val[5];
    
    while (1) {
      // Get the position of the line.
        current_pos_of_line = m3pi.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 
        m3pi.left_motor(left);
        m3pi.right_motor(right);
        
        //Marc's sensor test
        sensor_all(sensor_val); // sensor_value gets fkt value
        if (sensor_val[0] > 350 && sensor_val[4] > 350)
            {
                 
                m3pi.stop();
                turn_right();
                m3pi.stop();
                m3pi.forward(0.2);
                wait (1);
                m3pi.stop();
                m3pi.backward(0.2);
                wait (1);
                m3pi.stop();
                turn_left();
                m3pi.stop();
            }
        /*
        sensor_value4 = sensor_all(4); // sensor_value gets fkt value
        */
            
        /*
        m3pi.printf("%d", sensor_value0); // prints one of sidesensors value
        m3pi.locate(0,1);
        m3pi.printf("%d", sensor_value4); // prints the other sidesensor value
        */

    }
}