diff options
Diffstat (limited to 'exp/FB_PID.EXP')
| -rw-r--r-- | exp/FB_PID.EXP | 102 |
1 files changed, 102 insertions, 0 deletions
diff --git a/exp/FB_PID.EXP b/exp/FB_PID.EXP new file mode 100644 index 0000000..da3b92c --- /dev/null +++ b/exp/FB_PID.EXP @@ -0,0 +1,102 @@ +
+(* @NESTEDCOMMENTS := 'Yes' *)
+(* @PATH := '' *)
+(* @OBJECTFLAGS := '0, 8' *)
+(* @SYMFILEFLAGS := '2048' *)
+FUNCTION_BLOCK FB_PID
+VAR_INPUT (* ex.: positioning *)
+ IN_xEnable : BOOL := TRUE;
+ IN_xReset : BOOL;
+ IN_rW : REAL; (* ex -> target position *)
+ IN_rX : REAL; (* ex -> actual position *)
+
+ IN_rKp : REAL := 1.0;
+ IN_rKi : REAL := 1.0;
+ IN_rKd : REAL := 1.0;
+
+ IN_xLimit : BOOL := FALSE;
+ IN_rLimit : REAL := 1024;
+
+ IN_rTa : REAL; (* sampling rate in s *)
+END_VAR
+VAR_OUTPUT
+ OUT_rY : REAL; (* ex. -> set speed *)
+END_VAR
+VAR
+ rESum : REAL;
+ rE_old : REAL;
+ rE : REAL;
+ xWithinLimits : BOOL;
+ rY_tmp : REAL;
+ dwReset: DWORD;
+END_VAR
+
+(*
+
+PID CONTROLLER
+Author: mo
+Date: 2019-02
+ |z
+ w e y v
+ -->( )--->[controller]---->[system]---->
+ ^x |
+ | |
+ ----------------------------------
+
+Set up PID using Ziegler-Nichols-Method:
+
+1) Ki = 0, Kd = 0
+2) increase Kp until periodic oscillation occurs (and never stops swinging)
+3) this value is Kp_crit
+4) measured period length is T_crit
+5) use this table
+
+ Kp = 0.6 * Kp_crit
+ Tn = 0.5 * T_crit
+ Tv = 0.12 * T_crit
+
+ -> with [Ki = Kp / Tn] and [Kd = Kp * Tv]:
+
+ Kp = 0.6 * Kp_crit
+ Ki = Kp / (0.5 * T_crit)
+ Kd = Kp * (0.12 * T_crit)
+
+e.g.
+Kp_crit = 25
+T_crit = 2s
+
+Kp = 0.6 * 25 = 15
+Ki = 15 / (0.5 * 2s) = 15
+Kd = 15 * (0.12 * 2s) = 3,6
+
+*)
+
+(* @END_DECLARATION := '0' *)
+IF IN_xEnable
+ AND NOT IN_xReset
+THEN
+ rE := IN_rW - IN_rX;
+
+ xWithinLimits := NOT IN_xLimit
+ OR (IN_xLimit AND rY_tmp >= -IN_rLimit AND rY_tmp < IN_rLimit);
+
+ IF xWithinLimits THEN
+ rESum := rEsum + rE;
+ END_IF
+
+ rY_tmp := (IN_rKp*rE) + (IN_rKi*IN_rTa*rEsum) + (IN_rKd*(rE-rE_old)/IN_rTa);
+ OUT_rY := rY_tmp;
+
+ IF NOT xWithinLimits THEN
+ IF OUT_rY >= 0 THEN OUT_rY := IN_rLimit; ELSE OUT_rY := -IN_rLimit; END_IF
+ END_IF
+
+ rE_old := rE;
+END_IF
+
+IF IN_xReset THEN
+ rESum := 0;
+ rE_old := 0;
+ dwReset := dwReset + 1;
+END_IF
+END_FUNCTION_BLOCK
|