Peter Knoben / PIDController

PID

Dependents:   kinematics_control kinematics_controlv2 kinematics_controlv4 kinematics_control_copyfds ... more

PIDControl.cpp@1:c7f0b343df31, 2017-11-02 (annotated)

Committer:
peterknoben
Date:
Thu Nov 02 15:08:48 2017 +0000
Revision:
1:c7f0b343df31
Parent:
0:b076b5e36978 
Working

Who changed what in which revision?

UserRevisionLine numberNew contents of line
peterknoben 0:b076b5e36978 1 #include "PIDControl.h"
peterknoben 0:b076b5e36978 2 #include "mbed.h"
peterknoben 0:b076b5e36978 3
peterknoben 0:b076b5e36978 4 PIDControl::PIDControl(void)
peterknoben 0:b076b5e36978 5 {
peterknoben 0:b076b5e36978 6
peterknoben 0:b076b5e36978 7 }
peterknoben 0:b076b5e36978 8
peterknoben 1:c7f0b343df31 9 //------------------------------------------------------------------------------
peterknoben 0:b076b5e36978 10 //Calculate the value after PID control.
peterknoben 0:b076b5e36978 11 double PIDControl::get(float error, const float Kp, const float Ki, const float Kd, const float Ts, const float N, double &v1, double &v2) {
peterknoben 0:b076b5e36978 12 const double a1 = -4 / (N*Ts+2);
peterknoben 0:b076b5e36978 13 const double a2 = -(N*Ts-2) / ( N*Ts+2);
peterknoben 0:b076b5e36978 14 const double b0 = (4*Kp + 4*Kd*N +2*Ki*Ts + 2*Kp*N*Ts + Ki*N*pow(Ts,2)) / (2*N*Ts + 4);
peterknoben 0:b076b5e36978 15 const double b1 = (Ki*N*pow(Ts,2) - 4*Kp - 4*Kd*N) / (N*Ts + 2);
peterknoben 0:b076b5e36978 16 const double b2 = (4*Kp + 4*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*pow(Ts,2)) / (2*N*Ts + 4);
peterknoben 0:b076b5e36978 17
peterknoben 0:b076b5e36978 18 double v = error - a1*v1 - a2*v2;
peterknoben 0:b076b5e36978 19 double u = b0*v + b1*v1 + b2*v2;
peterknoben 0:b076b5e36978 20 v2=v1;
peterknoben 0:b076b5e36978 21 v1=v;
peterknoben 0:b076b5e36978 22 return u;
peterknoben 0:b076b5e36978 23 }
peterknoben 0:b076b5e36978 24