Michael Peter / OS 2 nucf446-GPA_V03_double

gpa in double prescision

Dependencies:   mbed

PI_Cntrl.cpp@7:b3c5116e9fab, 21 months ago (annotated)

Committer:
pmic
Date:
Fri Mar 02 17:11:34 2018 +0000
Revision:
7:b3c5116e9fab
Parent:
0:15be70d21d7c
Child:
12:e54941459353
additional filter implementation, corrections and get rid of some spaghetti code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rtlabor 0:15be70d21d7c 1 #include "PI_Cntrl.h"
rtlabor 0:15be70d21d7c 2
rtlabor 0:15be70d21d7c 3 using namespace std;
rtlabor 0:15be70d21d7c 4
rtlabor 0:15be70d21d7c 5 PI_Cntrl::PI_Cntrl(float Kp_, float Tn_){
rtlabor 0:15be70d21d7c 6 Kp = Kp_;
rtlabor 0:15be70d21d7c 7 Tn = Tn_;
rtlabor 0:15be70d21d7c 8 i_part_old = 0;
rtlabor 0:15be70d21d7c 9 del = 0;
rtlabor 0:15be70d21d7c 10 out_max =1e10;
rtlabor 0:15be70d21d7c 11 out_min=-1e10;
rtlabor 0:15be70d21d7c 12 }
rtlabor 0:15be70d21d7c 13
rtlabor 0:15be70d21d7c 14 PI_Cntrl::PI_Cntrl(float Kp_, float Tn_,float ma){
rtlabor 0:15be70d21d7c 15 Kp = Kp_;
rtlabor 0:15be70d21d7c 16 Tn = Tn_;
rtlabor 0:15be70d21d7c 17 i_part_old = 0;
rtlabor 0:15be70d21d7c 18 del = 0;
rtlabor 0:15be70d21d7c 19 out_max = ma;
rtlabor 0:15be70d21d7c 20 out_min = -ma;
rtlabor 0:15be70d21d7c 21 }
rtlabor 0:15be70d21d7c 22
rtlabor 0:15be70d21d7c 23 PI_Cntrl::~PI_Cntrl() {}
rtlabor 0:15be70d21d7c 24
rtlabor 0:15be70d21d7c 25 void PI_Cntrl::reset(float initValue) {
rtlabor 0:15be70d21d7c 26 i_part_old = initValue;
rtlabor 0:15be70d21d7c 27 del = 0;
rtlabor 0:15be70d21d7c 28 }
rtlabor 0:15be70d21d7c 29
rtlabor 0:15be70d21d7c 30 float PI_Cntrl::doStep(float error){
rtlabor 0:15be70d21d7c 31 float kpe = Kp * error;
pmic 7:b3c5116e9fab 32 float i_part = Ts/Tn*(kpe-1.0f*del) + i_part_old; // I with windup subtraction
pmic 7:b3c5116e9fab 33 i_part_old = i_part;
pmic 7:b3c5116e9fab 34 float y_out_temp = (kpe + i_part); // unconstrained output
rtlabor 0:15be70d21d7c 35 float y_out;
rtlabor 0:15be70d21d7c 36 // -------- limit output --------
rtlabor 0:15be70d21d7c 37 if(y_out_temp > out_max)
rtlabor 0:15be70d21d7c 38 y_out = out_max;
rtlabor 0:15be70d21d7c 39 else if(y_out_temp < out_min)
rtlabor 0:15be70d21d7c 40 y_out = out_min;
rtlabor 0:15be70d21d7c 41 else
rtlabor 0:15be70d21d7c 42 y_out = y_out_temp;
rtlabor 0:15be70d21d7c 43 // -------- Anti-Windup --------
rtlabor 0:15be70d21d7c 44 del = (y_out_temp - y_out);
rtlabor 0:15be70d21d7c 45 // -------- Timeshift --------
pmic 7:b3c5116e9fab 46
rtlabor 0:15be70d21d7c 47 return y_out;
rtlabor 0:15be70d21d7c 48 }