(* @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