
gpa in double prescision
PI_Cntrl.cpp@7:b3c5116e9fab, 2018-03-02 (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?
User | Revision | Line number | New 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 | } |