﻿<?xml version="1.0" encoding="utf-8"?>
<TcPlcObject Version="1.1.0.1">
  <POU Name="IncrementalEncoder" Id="{ac340f7f-a9a5-4e21-8ca2-a669282dc17e}" SpecialFunc="None">
    <Declaration><![CDATA[FUNCTION_BLOCK IncrementalEncoder
VAR_INPUT
	config : REFERENCE TO ConfigSheaveMeasurement;
END_VAR

VAR
	encoderInput		AT %I*  : PulseEncoderInput;;
{attribute 'hide'}	A_rise						: R_TRIG;
{attribute 'hide'}	A_fall						: F_TRIG;
{attribute 'hide'}	B_rise						: R_TRIG;
{attribute 'hide'}	B_fall						: F_TRIG;
{attribute 'hide'}	direction					: BOOL;
{attribute 'hide'}	count						: BOOL;		// any work to do?
{attribute 'hide'}	_countWarnings				: BOOL;		// wait until TRUE before counting errors
{attribute 'hide'}	msg							: DebugMessage_timed;	
{attribute 'hide'} _Last_warning_counter		: UDINT;	

	
END_VAR
VAR PERSISTENT
	_counterValue				: UDINT;		
	_warning_counter			: UDINT;		// counts the number of scans with more than one rising or falling edge		
END_VAR
]]></Declaration>
    <Implementation>
      <ST><![CDATA[_Last_warning_counter := _warning_counter;

// counter logic
A_rise(clk := encoderInput.A_pulse);
A_fall(clk := encoderInput.A_pulse);
B_rise(clk := encoderInput.B_pulse);
B_fall(clk := encoderInput.B_pulse);

count := FALSE;							// Only count if there's a rising or falling edge.
IF 	A_rise.Q THEN							// On any rising or falling edge, the state of the other pulse will determine the direction
	direction := NOT encoderInput.B_pulse;
	IF count AND _countWarnings THEN _warning_counter := _warning_counter +1; END_IF
	count := TRUE;
END_IF
IF 	A_fall.Q THEN
	direction := encoderInput.B_pulse;
	IF count AND _countWarnings THEN _warning_counter := _warning_counter +1; END_IF
	count := TRUE;
END_IF
IF 	B_rise.Q THEN
	direction := encoderInput.A_pulse;
	IF count AND _countWarnings THEN _warning_counter := _warning_counter +1; END_IF
	count := TRUE;
END_IF
IF 	B_fall.Q THEN
	direction := NOT encoderInput.A_pulse;
	IF count AND _countWarnings THEN _warning_counter := _warning_counter +1; END_IF
	count := TRUE;
END_IF

IF count THEN
	_counterValue := _counterValue + INT_TO_UDINT( SEL(direction,1,-1)) ; 		// add or subtract depending on direction bit
END_IF
_countWarnings := TRUE;

IF _Last_warning_counter <> _warning_counter THEN
	msg(st1 := config.name, st2 := 'Warning: Possible block counter error');
END_IF

]]></ST>
    </Implementation>
    <Property Name="revolutions" Id="{8ebc4831-8f17-4233-9074-f52bc68392aa}">
      <Declaration><![CDATA[{attribute 'monitoring' := 'call'}
{attribute 'TcRpcEnable'}
// number of sheave revolutions
PROPERTY revolutions : LREAL]]></Declaration>
      <Get Name="Get" Id="{4d5edf7d-2cd1-41a6-b848-946337a713e1}">
        <Declaration><![CDATA[VAR
END_VAR
]]></Declaration>
        <Implementation>
          <ST><![CDATA[revolutions := countValueSigned / DINT_TO_LREAL(config.SheaveEncoderResolution * 4);		// multiply by 4 since we're counting all edges]]></ST>
        </Implementation>
      </Get>
    </Property>
    <Property Name="countValue" Id="{cd7d6a80-9890-4540-81c6-d0bf2c3d8a98}">
      <Declaration><![CDATA[{attribute 'monitoring' := 'call'}
{attribute 'TcRpcEnable'}
// Raw counter value
PROPERTY countValue : UDINT]]></Declaration>
      <Get Name="Get" Id="{04891533-f2ee-452e-9e4d-00f2106ee7c6}">
        <Declaration><![CDATA[VAR
END_VAR
]]></Declaration>
        <Implementation>
          <ST><![CDATA[countValue := _counterValue;]]></ST>
        </Implementation>
      </Get>
    </Property>
    <Property Name="countValueSigned" Id="{eb5a1503-143e-4455-b394-1051e8b9fa2b}">
      <Declaration><![CDATA[{attribute 'monitoring' := 'call'}
{attribute 'TcRpcEnable'}
// Raw counter value
PROPERTY countValueSigned : DINT]]></Declaration>
      <Get Name="Get" Id="{32c27a93-dc44-49b4-93fb-833aedf52394}">
        <Declaration><![CDATA[VAR
END_VAR
]]></Declaration>
        <Implementation>
          <ST><![CDATA[countValueSigned :=  UDINT_TO_DINT(_counterValue);]]></ST>
        </Implementation>
      </Get>
    </Property>
    <Method Name="reset" Id="{fe96c867-7b3f-49b4-8859-511dd35272be}">
      <Declaration><![CDATA[//Set counter and warning counter to Zero
METHOD PUBLIC reset : BOOL

]]></Declaration>
      <Implementation>
        <ST><![CDATA[_counterValue := 0;		
_warning_counter := 0;	]]></ST>
      </Implementation>
    </Method>
  </POU>
</TcPlcObject>