RT2_Cuboid_demo / Cntrl_Lib

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:  

Fork of Cntrl_Lib by Ruprecht Altenburger

PID_Cntrl.cpp@1:bf62e74fbcf3, 2018-09-28 (annotated)

Committer:
altb
Date:
Fri Sep 28 09:01:42 2018 +0000
Revision:
1:bf62e74fbcf3
Parent:
0:e2a7d7f91e49
Child:
2:1f8ddc46c578
.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb 0:e2a7d7f91e49 1 /*
altb 0:e2a7d7f91e49 2 PI Controller class with anti windup reset in biquad transposed direct form 2
altb 0:e2a7d7f91e49 3 see e.g.: https://www.dsprelated.com/freebooks/filters/Four_Direct_Forms.html
altb 0:e2a7d7f91e49 4 everything is calculated in double
altb 0:e2a7d7f91e49 5
altb 1:bf62e74fbcf3 6 Ts z - 1
altb 1:bf62e74fbcf3 7 G(s) = P + I ------- + D -------
altb 1:bf62e74fbcf3 8 z - 1 z - p
altb 0:e2a7d7f91e49 9 */
altb 0:e2a7d7f91e49 10
altb 1:bf62e74fbcf3 11 #include "PID_Cntrl.h"
altb 0:e2a7d7f91e49 12
altb 0:e2a7d7f91e49 13 using namespace std;
altb 0:e2a7d7f91e49 14
altb 1:bf62e74fbcf3 15 PID_Cntrl::PID_Cntrl(float P, float I, float D, float tau_f, float Ts, float uMax, float uMin)
altb 0:e2a7d7f91e49 16 {
altb 1:bf62e74fbcf3 17 setCoefficients(P, I, D, tau_f, Ts);
altb 0:e2a7d7f91e49 18 this->uMax = (double)uMax;
altb 0:e2a7d7f91e49 19 this->uMin = (double)uMin;
altb 0:e2a7d7f91e49 20 reset(0.0f);
altb 0:e2a7d7f91e49 21 }
altb 0:e2a7d7f91e49 22
altb 1:bf62e74fbcf3 23 PID_Cntrl::~PID_Cntrl() {}
altb 0:e2a7d7f91e49 24
altb 1:bf62e74fbcf3 25 void PID_Cntrl::reset(float initValue)
altb 0:e2a7d7f91e49 26 {
altb 1:bf62e74fbcf3 27 Iold = (double)initValue;
altb 1:bf62e74fbcf3 28 eold = 0.0;yold = 0.0;
altb 1:bf62e74fbcf3 29 del = 0.0;
altb 0:e2a7d7f91e49 30 }
altb 0:e2a7d7f91e49 31
altb 1:bf62e74fbcf3 32 void PID_Cntrl::setCoefficients(float P, float I, float D, float tau_f, float Ts)
altb 0:e2a7d7f91e49 33 {
altb 1:bf62e74fbcf3 34 this->p = 1.0 - (double)Ts/(double)tau_f;
altb 1:bf62e74fbcf3 35 this->P = P;
altb 1:bf62e74fbcf3 36 this->I = I;
altb 1:bf62e74fbcf3 37 this->D = D;
altb 1:bf62e74fbcf3 38
altb 0:e2a7d7f91e49 39 }
altb 0:e2a7d7f91e49 40
altb 1:bf62e74fbcf3 41 float PID_Cntrl::doStep(double e)
altb 0:e2a7d7f91e49 42 {
altb 1:bf62e74fbcf3 43 double u = P*e + D*(e-eold)+p*yold +Iold+I*Ts*(e-del); // unconstrained output
altb 0:e2a7d7f91e49 44 double uc = u; // constrained output
altb 0:e2a7d7f91e49 45 if(u > uMax) uc = uMax;
altb 0:e2a7d7f91e49 46 else if(u < uMin) uc = uMin;
altb 1:bf62e74fbcf3 47 del=(u-uc)/P;
altb 0:e2a7d7f91e49 48 return (float)uc;
altb 0:e2a7d7f91e49 49 }