RT2_Cuboid_demo / Mbed 2 deprecated nucf446-test_GPA

Test of pmic GPA with filter

Dependencies:   mbed

Fork of nucf446-cuboid-balance1_strong by RT2_Cuboid_demo

PI_Cntrl.cpp@0:15be70d21d7c, 2018-01-10 (annotated)

Committer:
rtlabor
Date:
Wed Jan 10 16:08:07 2018 +0000
Revision:
0:15be70d21d7c
Child:
5:d6c7ccbbce78
balance cuboid

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;
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 }