File content as of revision 14:e5fc69651b1d:

#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)
    B[0] = 0.0640; // b0
    B[1] = 0.1279; // b1
    B[2] = 0.0640; // b2
    // a0 = 1
    A[0] = -1.1683; // a1
    A[1] = 0.4241; // a2
}

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;
}

void PID::clear_state() {
    first_update = true;
    error_previous = 0;
    error_integral = 0;
    
    // Cleart the low pass filter.
    wz[0] = 0;
    wz[1] = 0;
}

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 (needs filter)
    double error_der = (error-error_previous)/pid_period;
    double filtered_error_der = biquad_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;
}

double PID::biquad_step(double x) {

    double y,w;

    /* Direct form II */
    w =      x - A[0]*wz[0] - A[1]*wz[1];
    y = B[0]*w + B[1]*wz[0] + B[2]*wz[1];

    /* Shift */
    wz[1] = wz[0];
    wz[0] = w;

    return y;
}