Peter Knoben / PIDController

PID

Dependents:   kinematics_control kinematics_controlv2 kinematics_controlv4 kinematics_control_copyfds ... more

PIDControl.cpp

Committer:
peterknoben
Date:
2017-10-26
Revision:
0:b076b5e36978
Child:
1:c7f0b343df31

File content as of revision 0:b076b5e36978:

#include "PIDControl.h"
#include "mbed.h"

PIDControl::PIDControl(void)
{

}

//Calculate the value after PID control. 
double PIDControl::get(float error, const float Kp, const float Ki, const float Kd, const float Ts, const float N, double &v1, double &v2) {
    const double a1 = -4 / (N*Ts+2); 
    const double a2 = -(N*Ts-2) / ( N*Ts+2);
    const double b0 = (4*Kp + 4*Kd*N +2*Ki*Ts + 2*Kp*N*Ts + Ki*N*pow(Ts,2)) / (2*N*Ts + 4);
    const double b1 = (Ki*N*pow(Ts,2) - 4*Kp - 4*Kd*N) / (N*Ts + 2);
    const double b2 = (4*Kp + 4*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*pow(Ts,2)) / (2*N*Ts + 4);
    
    double v = error - a1*v1 - a2*v2;
    double u = b0*v + b1*v1 + b2*v2;
    v2=v1;
    v1=v;
    return u;  
}