File content as of revision 9:2779500685cb:

#include "PID.h"

PIDControll::PIDControll() { 
    //variabelen instellen als we ons PIDControll object aanmaken
    Kp = 10;
    Ki = 0.0001; 
    Kd = 5;
    set_point = 0;         // camera hoek
    proportional = 0;
    last_proportional =0;
    integral = 0;
    derivative = 0;
    errorPID = 0;  
    dir = 0;  
}

void PIDControll::PIDaanpassing(float angle){
    proportional = set_point - angle;        
    integral += proportional; 
    derivative = proportional - last_proportional;
    last_proportional = proportional;
    
    errorPID = (Kp * proportional) + (Ki * integral) + (Kd * derivative); 
    (errorPID > 0)?(dir = 1):(dir = 0);
    
    // errorPID is restricted between -400 and 400 
    if(errorPID > 400)
        errorPID = 400;
    else if(errorPID < -400)
        errorPID = -400;   
   
   stop = 0;   
   delay = 0.1/abs(errorPID);      // speed should be proportional to error, therefore time delay
                                     //between steps should be inverse proportional to error.
   if (abs(errorPID)< Kp/2) stop = 1;  // 0.5 deg noise margin
         
}