FUNCTION_BLOCK FB20 // Bewegung erkennen
VAR_INPUT
Position:INT; // Daten
END_VAR
VAR_OUTPUT
Vor:BOOL; // Antrieb fährt vor
Rueck:BOOL; // Antrieb fährt zurück
Speed:REAL; // Geschwindigkeit in mm/s
END_VAR
VAR
Sys_Time_akt:TIME; // Systemzeit
Sys_Time_old:TIME; // Systemzeit
Cycletime:TIME;
Cycletime2:TIME;
Daten:ARRAY [1..11] OF INT;
Mittel:DINT;
END_VAR
VAR_TEMP
i:INT;
bie:BOOL;
END_VAR
LABEL
END_LABEL
BEGIN
Sys_Time_akt:=sfc64();
IF Sys_Time_akt < T#100ms AND Sys_Time_old > t#2147483000ms THEN
Sys_time_Old:= Sys_time_Old - T#2147483647ms;
END_IF;
Cycletime:= Sys_time_Akt - Sys_time_Old; // Zeit seit letztem negativen Flankenwechsel vom Signal
Sys_time_Old:=Sys_time_Akt;
Cycletime2:=Cycletime2 + Cycletime;
// Takt 100ms - Werte ins Array schreiben
IF Cycletime2 >= t#100ms THEN
IF Cycletime2 >= T#200ms THEN
Cycletime2:=t#100ms;
END_IF;
Cycletime2:=Cycletime2 - T#100ms;
// Daten wegspeichern
FOR i:= 9 TO 1 BY - 1 DO
Daten[i+1]:=daten[i];
END_FOR;
Daten[1]:=position - daten[11];
daten[11]:=position;
Mittel:=0;
FOR i:= 1 TO 10 DO
Mittel:=Mittel + INT_TO_DINT(daten[i]);
END_FOR;
IF mittel = 0 THEN
Speed:=0.0;
ELSE
Speed:=( Speed + DINT_TO_REAL(Mittel) ) / 2.0 ;
END_IF;
END_IF;
// Motion detection - vor/zurück
IF speed > 0.0 THEN
vor:=true;
rueck:=false;
Bie:=true;
ELSIF speed < 0.0 THEN
vor:=false;
rueck:=true;
Bie:=true;
ELSe
vor:=false;
rueck:=false;
Bie:=false;
END_IF;
OK:=bie;
END_FUNCTION_BLOCK