path: root/lib61131aux.lib

CoDeSys+�	bu\
ATlcpld\VAR_GLOBAL
END_VAR

'��Globale_Variablenbu\	bu\eTe ARYPVAR_GLOBAL
END_VAR
,=
FB_BECKHOFF_EL5101_INCbu\	bu\�
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

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

a_Referenzieren();

UArWert := E_CounterValue / UErSkalierung;
#a_Referenzierenbu\|
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$,FB_DIbu\	bu\�
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_VARH(* 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;,\FB_EWMAbu\	bu\�
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

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

OUT_rXk_avg :=	rAlpha * rXk_avg_old
			+ ( (1 - rAlpha) * IN_rXk);

rXk_avg_old := OUT_rXk_avg;
!,KK�
|FB_INC_DECODERbu\	bu\
N_ :0.QFUNCTION_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    _|    |____|    |____|

*)
JrtChA(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;",dd��FB_PIDbu\	bu\R ge	eeb�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

*)
'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 ,KK��MAINbu\	bu\T;ENVA
]PROGRAM MAIN
VAR
END_VAR
;����lo\PCO\;
"STANDARD.LIB 5.6.98 11:03:02@V�w5CONCAT@	CTD@	CTU@
CTUD@DELETE@F_TRIG@
FIND@INSERT@
LEFT@	LEN@	MID@R_TRIG@
REPLACE@RIGHT@RS@
SEMA@SR@	TOF@	TON@TP@
Global Variables 0@


	Bausteine


TEMP
FB_BECKHOFF_EL5101_INC


a_Referenzieren#

FB_DI$����
FB_EWMA
FB_INC_DECODER!
FB_PID"
MAIN ����




Datentypen����


Globale Variablen

Globale_Variablen����



Visualisierungen����@
fu\B]��