Wik de Haas / Mbed 2 deprecated Aansturing_knoppen

Aansturing

Dependencies:   Encoder mbed HIDScope

PID.cpp

Committer:
wikdehaas
Date:
2016-11-03
Revision:
3:57b98989b0b1
Parent:
0:55f9447aa02b

File content as of revision 3:57b98989b0b1:

#include "PID.h"

double PID( double err, const double Kp, const double Ki, const double Kd, const double Ts, const double N, double &v1, double &v2 ) {
// These variables are only calculated once!
const double a1 = -4/(N*Ts+2), 
a2 = -(N*Ts-2)/(N*Ts+2), 
b0 = (4*Kp + 4*Kd*N + 2*Ki*Ts + 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4), 
b1 = (Ki*N*pow(Ts,2) - 4*Kp - 4*Kd*N)/(N*Ts + 2),
b2 = (4*Kp + 4*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4);

double v = err - a1*v1 - a2*v2;
double u = b0*v + b1*v1 + b2*v2;
v2 = v1; v1 = v;
return u;
}