altb_pmic / Mbed 2 deprecated Test_Realbot

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.

Dependencies:   mbed

PT1_Controller.cpp@5:4c03a1a0ad98, 2020-03-10 (annotated)

Committer:
pmic
Date:
Tue Mar 10 09:14:00 2020 +0000
Revision:
5:4c03a1a0ad98
Parent:
0:937360eb9f8c 
Latest commit.;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pmic 0:937360eb9f8c 1 #include "PT1_Controller.h"
pmic 0:937360eb9f8c 2
pmic 0:937360eb9f8c 3 PT1_Controller::PT1_Controller(double Kp, double Ts, double Tf)
pmic 0:937360eb9f8c 4 {
pmic 0:937360eb9f8c 5 copyControllerPara(Kp, Ts, Tf);
pmic 0:937360eb9f8c 6 calcFilterCoeff();
pmic 0:937360eb9f8c 7 u_max = 1000000.0; // big number
pmic 0:937360eb9f8c 8 u_min = -u_max;
pmic 0:937360eb9f8c 9 }
pmic 0:937360eb9f8c 10
pmic 0:937360eb9f8c 11 PT1_Controller::PT1_Controller(double Kp, double Ts, double Tf, double u_max)
pmic 0:937360eb9f8c 12 {
pmic 0:937360eb9f8c 13 copyControllerPara(Kp, Ts, Tf);
pmic 0:937360eb9f8c 14 calcFilterCoeff();
pmic 0:937360eb9f8c 15 this->u_max = u_max;
pmic 0:937360eb9f8c 16 u_min = -u_max;
pmic 0:937360eb9f8c 17 }
pmic 0:937360eb9f8c 18
pmic 0:937360eb9f8c 19 PT1_Controller::PT1_Controller(double Kp, double Ts, double Tf, double u_max, double u_min)
pmic 0:937360eb9f8c 20 {
pmic 0:937360eb9f8c 21 copyControllerPara(Kp, Ts, Tf);
pmic 0:937360eb9f8c 22 calcFilterCoeff();
pmic 0:937360eb9f8c 23 this->u_max = u_max;
pmic 0:937360eb9f8c 24 this->u_min = u_min;
pmic 0:937360eb9f8c 25 }
pmic 0:937360eb9f8c 26
pmic 0:937360eb9f8c 27 PT1_Controller::~PT1_Controller() {}
pmic 0:937360eb9f8c 28
pmic 0:937360eb9f8c 29 void PT1_Controller::reset()
pmic 0:937360eb9f8c 30 {
pmic 0:937360eb9f8c 31 u_kmin1 = 0.0;
pmic 0:937360eb9f8c 32 uf_k = 0.0;
pmic 0:937360eb9f8c 33 }
pmic 0:937360eb9f8c 34
pmic 0:937360eb9f8c 35 void PT1_Controller::reset(double setVal)
pmic 0:937360eb9f8c 36 {
pmic 0:937360eb9f8c 37 u_kmin1 = setVal;
pmic 0:937360eb9f8c 38 uf_k = setVal;
pmic 0:937360eb9f8c 39 }
pmic 0:937360eb9f8c 40
pmic 0:937360eb9f8c 41 double PT1_Controller::update(double e)
pmic 0:937360eb9f8c 42 {
pmic 0:937360eb9f8c 43 // proportional controller
pmic 0:937360eb9f8c 44 double u = Kp * e;
pmic 0:937360eb9f8c 45
pmic 0:937360eb9f8c 46 // constrained low-pass filter
pmic 0:937360eb9f8c 47 uf_k = u*bf + u_kmin1*bf - af*uf_k;
pmic 0:937360eb9f8c 48 u_kmin1 = u;
pmic 0:937360eb9f8c 49 return saturate(uf_k);
pmic 0:937360eb9f8c 50 }
pmic 0:937360eb9f8c 51
pmic 0:937360eb9f8c 52 void PT1_Controller::copyControllerPara(double Kp, double Ts, double Tf)
pmic 0:937360eb9f8c 53 {
pmic 0:937360eb9f8c 54 this->Kp = Kp;
pmic 0:937360eb9f8c 55 this->Ts = Ts;
pmic 0:937360eb9f8c 56 this->Tf = Tf;
pmic 0:937360eb9f8c 57 }
pmic 0:937360eb9f8c 58
pmic 0:937360eb9f8c 59 void PT1_Controller::calcFilterCoeff()
pmic 0:937360eb9f8c 60 {
pmic 0:937360eb9f8c 61 // tustin transformation
pmic 0:937360eb9f8c 62 bf = Ts / (2.0*Tf + Ts);
pmic 0:937360eb9f8c 63 af = (Ts - 2.0*Tf) / (2.0*Tf + Ts);
pmic 0:937360eb9f8c 64 }
pmic 0:937360eb9f8c 65
pmic 0:937360eb9f8c 66 double PT1_Controller::saturate(double u)
pmic 0:937360eb9f8c 67 {
pmic 0:937360eb9f8c 68 if(u > u_max) {
pmic 0:937360eb9f8c 69 u = u_max;
pmic 0:937360eb9f8c 70 } else if(u < u_min) {
pmic 0:937360eb9f8c 71 u = u_min;
pmic 0:937360eb9f8c 72 }
pmic 0:937360eb9f8c 73 return u;
pmic 0:937360eb9f8c 74 }
pmic 0:937360eb9f8c 75