File content as of revision 0:009e84d7af32:

#include "pid.h"

PID::PID() {
    Kp = 0;
    Ki = 0;
    Kd = 0;
    error_integral = 0;
    error_previous = 0;
    pid_period = 0;
    first_update = true;
    
    // Setup the low pass filter.
    // The values are from the slides (PES, lecture 5)
    low_pass_filter = BiQuad(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
}

void PID::set_period(double period) {
    pid_period = period;
}

void PID::set_k_values(double Kp, double Ki, double Kd) {
    this->Kp = Kp;
    this->Ki = Ki;
    this->Kd = Kd;
}

double PID::update(double error) {
    if (first_update) {
        // Make sure the previous error has a sensible value (not 0).
        error_previous = error;
        first_update = false;
    }
    
    // Proportional part
    double u_p = Kp * error;
    
    // Integral part
    error_integral += error * pid_period;
    double u_i = Ki * error_integral;
    
    // Derivative part
    double error_der = (error-error_previous)/pid_period;
    double filtered_error_der = low_pass_filter.step(error_der);
    double u_d = Kd * filtered_error_der;
    error_previous = error;
    
    // Add all components and return.
    return u_p + u_d + u_i;
}