add *.exp; (this is redundant information, just to be able to diff later on)HEADmaster

6 files changed, 411 insertions, 0 deletions

diff --git a/exp/FB_BECKHOFF_EL5101_INC.EXP b/exp/FB_BECKHOFF_EL5101_INC.EXP
new file mode 100644
index 0000000..e262631
--- /dev/null
+++ b/exp/FB_BECKHOFF_EL5101_INC.EXP
@@ -0,0 +1,62 @@
+

+(* @NESTEDCOMMENTS := 'Yes' *)

+(* @PATH := '\/TEMP' *)

+(* @OBJECTFLAGS := '0, 8' *)

+(* @SYMFILEFLAGS := '2048' *)

+FUNCTION_BLOCK FB_BECKHOFF_EL5101_INC

+

+VAR_INPUT

+	E_InfoData_AdsAddr_port		AT%I*	: INT;

+	E_InfoData_State			AT%I*	: INT;

+

+	E_CounterValue				AT%I*	: UINT;

+	UEboReferenzieren					: BOOL;

+	UErSkalierung						: REAL;

+	UErRefWertMm						: REAL;

+END_VAR

+

+VAR_OUTPUT

+	A_SetCounter				AT%Q*	: BOOL;

+	A_SetCounterValue			AT%Q*	: UINT;

+	UAboReferenziert					: BOOL;

+	UArWert: REAL;

+END_VAR

+

+VAR

+	pfReferenzieren						: R_TRIG;

+END_VAR

+

+

+(* @END_DECLARATION := '0' *)

+(***************************************************)

+(** Beckhoff EL5101 Incremental Encoder Interface **)

+(***************************************************)

+

+a_Referenzieren();

+

+UArWert := E_CounterValue / UErSkalierung;

+END_FUNCTION_BLOCK

+ACTION	a_Referenzieren:

+pfReferenzieren(clk:=

+	UEboReferenzieren

+	AND E_InfoData_State = 8	(* Op *)

+);

+UEboReferenzieren := FALSE;

+

+

+A_SetCounter := 0;

+(* z.B.	RefWertMm = 432

+		Skalierung = 25

+	=> 10800			*)

+A_SetCounterValue :=

+	REAL_TO_UINT(UErRefWertMm

+	* UErSkalierung);

+	(* TODO: Andere Typen? *)

+

+

+IF pfReferenzieren.Q THEN

+	A_SetCounter := 1;

+	UAboReferenziert := TRUE;

+END_IF

+END_ACTION

+

diff --git a/exp/FB_DI.EXP b/exp/FB_DI.EXP
new file mode 100644
index 0000000..5bead42
--- /dev/null
+++ b/exp/FB_DI.EXP
@@ -0,0 +1,68 @@
+

+(* @NESTEDCOMMENTS := 'Yes' *)

+(* @PATH := '\/TEMP' *)

+(* @OBJECTFLAGS := '0, 8' *)

+(* @SYMFILEFLAGS := '2048' *)

+FUNCTION_BLOCK FB_DI

+

+VAR_INPUT

+	E_INPUT	AT%I*	: BOOL;

+	UEboDisable	: BOOL;

+	UEboResetFlankenZaehler: BOOL;

+	UEtEntprellZeit: TIME := t#50ms;

+	UEboProtokolle : BOOL;

+	UEsName: STRING := 'unbenannt';

+END_VAR

+

+VAR_OUTPUT

+	UAboInputPosFlanke: BOOL;

+	UAboInputEntprellt: BOOL;

+	UAboInput: BOOL;

+	UAboInputNegFlanke: BOOL;

+END_VAR

+

+VAR

+	pfInput: R_TRIG;

+	dwPosFlanken: DWORD;

+	tonInput: TON;

+	nfInput: F_TRIG;

+	dwNegFlanken: DWORD;

+	sTemp: STRING;

+END_VAR

+(* @END_DECLARATION := '0' *)

+(* Digital Input FB *)

+

+pfInput(clk:=E_INPUT AND NOT UEboDisable);

+nfInput(clk:=E_INPUT AND NOT UEboDisable);

+

+IF pfInput.Q THEN

+	dwPosFlanken := dwPosFlanken + 1;

+(*	IF UEboProtokolle THEN

+		fProtokoll(1, CONCAT('pfInput: ', UEsName), 0);

+	END_IF*)

+END_IF

+

+IF nfInput.Q THEN

+	dwNegFlanken := dwNegFlanken + 1;

+(*	IF UEboProtokolle THEN

+		fProtokoll(1, CONCAT('nfInput: ', UEsName), 0);

+	END_IF*)

+END_IF

+

+IF UEboResetFlankenZaehler THEN

+	UEboResetFlankenZaehler := FALSE;

+	dwPosFlanken := 0;

+	dwNegFlanken := 0;

+END_IF

+

+(************************************)

+

+tonInput(in:=E_INPUT AND NOT UEboDisable, pt:=UEtEntprellZeit);

+

+(************************************)

+

+UAboInput := E_INPUT AND NOT UEboDisable;

+UAboInputPosFlanke := pfInput.Q;

+UAboInputNegFlanke := nfInput.Q;

+UAboInputEntprellt := tonInput.Q;

+END_FUNCTION_BLOCK

diff --git a/exp/FB_EWMA.EXP b/exp/FB_EWMA.EXP
new file mode 100644
index 0000000..6496df9
--- /dev/null
+++ b/exp/FB_EWMA.EXP
@@ -0,0 +1,35 @@
+

+(* @NESTEDCOMMENTS := 'Yes' *)

+(* @PATH := '' *)

+(* @OBJECTFLAGS := '0, 8' *)

+(* @SYMFILEFLAGS := '2048' *)

+FUNCTION_BLOCK FB_EWMA

+VAR_INPUT

+	IN_rN			: REAL := 15;	(* Filter constant alpha := rN/(rN+1) *)

+	IN_rXk			: REAL;			(* Input value to smooth *)

+END_VAR

+VAR_OUTPUT

+	OUT_rXk_avg		: REAL;		(* smoothed output *)

+END_VAR

+VAR

+	rAlpha			: REAL;

+	rXk_avg_old		: REAL;

+END_VAR

+

+(* Exponentially Weighted Moving Average Filter

+Author: mo

+Date: 2019-02

+

+Filter constant:	alpha := n / (n+1)

+EWMA Filter:		Xk_avg := alpha*Xk_prev + (1-alpha)*Xk

+

+*)

+(* @END_DECLARATION := '0' *)

+rAlpha := IN_rN/(IN_rN+1);

+

+OUT_rXk_avg :=	rAlpha * rXk_avg_old

+			+ ( (1 - rAlpha) * IN_rXk);

+

+rXk_avg_old := OUT_rXk_avg;

+

+END_FUNCTION_BLOCK

diff --git a/exp/FB_INC_DECODER.EXP b/exp/FB_INC_DECODER.EXP
new file mode 100644
index 0000000..2433c18
--- /dev/null
+++ b/exp/FB_INC_DECODER.EXP
@@ -0,0 +1,133 @@
+

+(* @NESTEDCOMMENTS := 'Yes' *)

+(* @PATH := '' *)

+(* @OBJECTFLAGS := '0, 8' *)

+(* @SYMFILEFLAGS := '2048' *)

+FUNCTION_BLOCK FB_INC_DECODER

+VAR_INPUT

+	IN_xChA			: BOOL;		(* Channel A *)

+	IN_xChB			: BOOL;		(* Channel B, 90-degree rotated to Channel A *)

+	IN_xReset		: BOOL;		(* Reset counter to 0 *)

+

+	IN_xEnPosWindow	: BOOL; 	(* TRUE if you want to overflow the counting at min/max values below *)

+	IN_diMinVal		: DINT;

+	IN_diMaxVal		: DINT;

+END_VAR

+VAR_OUTPUT

+	OUT_diActVal	: DINT;

+END_VAR

+VAR_OUTPUT PERSISTENT

+	OUT_pdiActVal	: DINT;

+END_VAR

+VAR

+	rtChA		: R_TRIG;

+	rtChB		: R_TRIG;

+	ftChA		: F_TRIG;

+	ftChB		: F_TRIG;

+	xFirstCycle	: BOOL;

+END_VAR

+

+(*

+

+Incremental Encoder Decoder FB

+Author: mo

+Date: 2019-02

+

+	alternative OSCAT implementation (faulty!) below:

+	

+	<OSCAT>

+	axb := cha XOR chb;

+	

+	(* create pulses for channel a *)

+	clka := cha XOR edgea;

+	edgea := cha;

+	

+	clkb := chb XOR edgeb;

+	edgeb := chb;

+	

+	(* create pulses for both channels *)

+	clk := clka OR clkb;

+	

+	(* set the direction output *)

+	IF axb AND clka THEN dir := TRUE; END_IF;

+	IF axb AND clkb THEN dir := FALSE; END_IF;

+	

+	(* increment or decrement the counter *)

+	IF clk AND dir THEN cnt := cnt + 1; END_IF;

+	IF clk AND NOT dir THEN cnt := cnt -1; END_IF;

+	

+	(* reset the counter if rst active *)

+	IF rst THEN cnt := 0; END_IF;

+	</OSCAT>

+

+=> problem here is: if your first pulse is e.g. a rising edge on CHb

+   with a high signal on CHa, it will increment cnt, but in the FALSE

+   direction instead of the TRUE direction. so your first pulse might count

+   into the wrong direction!

+

+

+This implementation accounts for all possible transitions.

+You can use OUT_diActVal or the persistent Version OUT_pdiActVal if your hardware

+is not moving while powered off to avoid reference runs at each restart of the PLC.

+

+  1  |----|    |----|    |-

+A 0 _|    |____|    |____|

+

+  1     |----|    |----|    |-

+B 0    _|    |____|    |____|

+

+*)

+

+(* @END_DECLARATION := '0' *)

+rtChA(clk:=IN_xChA);

+rtChB(clk:=IN_xChB);

+ftChA(clk:=IN_xChA);

+ftChB(clk:=IN_xChB);

+

+(* don't evaluate the changing edges when PLC starts up *)

+IF xFirstCycle THEN

+	IF 		(rtChA.Q AND NOT IN_xChB)

+		OR	(ftChA.Q AND IN_xChB)

+		OR	(rtChB.Q AND IN_xChA)

+		OR	(ftChB.Q AND NOT IN_xChA)

+	THEN

+		OUT_diActVal := OUT_diActVal + 1;

+		OUT_pdiActVal := OUT_pdiActVal + 1;

+	END_IF

+

+	IF		(rtChA.Q AND IN_xChB)

+		OR	(ftChA.Q AND NOT IN_xChB)

+		OR	(rtChB.Q AND NOT IN_xChA)

+		OR	(ftChB.Q AND IN_xChA)

+	THEN

+		OUT_diActVal := OUT_diActVal - 1;

+		OUT_pdiActVal := OUT_pdiActVal - 1;

+	END_IF

+END_IF

+

+IF IN_xReset THEN

+	OUT_diActVal := 0;

+	OUT_pdiActVal := 0;

+END_IF

+

+IF IN_xEnPosWindow THEN

+	IF OUT_diActVal < IN_diMinVal THEN

+		OUT_diActVal := IN_diMaxVal - 1;

+	END_IF

+

+	IF OUT_diActVal >= IN_diMaxVal THEN

+		OUT_diActVal := IN_diMinVal;

+	END_IF

+

+	(* for persistent counter *)

+	IF OUT_pdiActVal < IN_diMinVal THEN

+		OUT_pdiActVal := IN_diMaxVal - 1;

+	END_IF

+

+	IF OUT_pdiActVal >= IN_diMaxVal THEN

+		OUT_pdiActVal := IN_diMinVal;

+	END_IF

+END_IF

+

+xFirstCycle := TRUE;

+END_FUNCTION_BLOCK

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

diff --git a/exp/MAIN.EXP b/exp/MAIN.EXP
new file mode 100644
index 0000000..6ca487d
--- /dev/null
+++ b/exp/MAIN.EXP
@@ -0,0 +1,11 @@
+

+(* @NESTEDCOMMENTS := 'Yes' *)

+(* @PATH := '' *)

+(* @OBJECTFLAGS := '0, 8' *)

+(* @SYMFILEFLAGS := '2048' *)

+PROGRAM MAIN

+VAR

+END_VAR

+(* @END_DECLARATION := '0' *)

+;

+END_PROGRAM