From 39765e002e71a2dd54e932469877acfc1b6fcb67 Mon Sep 17 00:00:00 2001 From: pux Date: Thu, 7 Jan 2021 19:39:17 +0100 Subject: add *.exp; (this is redundant information, just to be able to diff later on) --- exp/FB_BECKHOFF_EL5101_INC.EXP | 62 +++++++++++++++++++ exp/FB_DI.EXP | 68 +++++++++++++++++++++ exp/FB_EWMA.EXP | 35 +++++++++++ exp/FB_INC_DECODER.EXP | 133 +++++++++++++++++++++++++++++++++++++++++ exp/FB_PID.EXP | 102 +++++++++++++++++++++++++++++++ exp/MAIN.EXP | 11 ++++ 6 files changed, 411 insertions(+) create mode 100644 exp/FB_BECKHOFF_EL5101_INC.EXP create mode 100644 exp/FB_DI.EXP create mode 100644 exp/FB_EWMA.EXP create mode 100644 exp/FB_INC_DECODER.EXP create mode 100644 exp/FB_PID.EXP create mode 100644 exp/MAIN.EXP (limited to 'exp') 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: + + + 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; + + +=> 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 -- cgit v1.2.2-1-g5e49