cuboid strong
Dependencies: mbed
PI_Cntrl.cpp@2:38d9d1c3c85f, 2018-03-01 (annotated)
- Committer:
- altb
- Date:
- Thu Mar 01 10:33:19 2018 +0000
- Revision:
- 2:38d9d1c3c85f
- Parent:
- 0:15be70d21d7c
Cuboid with lift up and down
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; |
rtlabor | 0:15be70d21d7c | 32 | float i_part = 1.0f/Tn * Ts * (kpe-del) + i_part_old; // I with windup subtraction |
rtlabor | 0:15be70d21d7c | 33 | float y_out_temp = (kpe + i_part_old ); // temporary variable |
rtlabor | 0:15be70d21d7c | 34 | float y_out; |
rtlabor | 0:15be70d21d7c | 35 | // -------- limit output -------- |
rtlabor | 0:15be70d21d7c | 36 | if(y_out_temp > out_max) |
rtlabor | 0:15be70d21d7c | 37 | y_out = out_max; |
rtlabor | 0:15be70d21d7c | 38 | else if(y_out_temp < out_min) |
rtlabor | 0:15be70d21d7c | 39 | y_out = out_min; |
rtlabor | 0:15be70d21d7c | 40 | else |
rtlabor | 0:15be70d21d7c | 41 | y_out = y_out_temp; |
rtlabor | 0:15be70d21d7c | 42 | // -------- Anti-Windup -------- |
rtlabor | 0:15be70d21d7c | 43 | del = (y_out_temp - y_out); |
rtlabor | 0:15be70d21d7c | 44 | // -------- Timeshift -------- |
rtlabor | 0:15be70d21d7c | 45 | i_part_old = i_part; |
rtlabor | 0:15be70d21d7c | 46 | return y_out; |
rtlabor | 0:15be70d21d7c | 47 | } |