<?xml version="1.0" encoding="utf-8"?>
<TcPlcObject Version="1.1.0.1" ProductVersion="3.1.4024.0">
  <POU Name="FB_AxisSim" Id="{5b3e4e46-5ace-4e6c-a0dc-d55113a89212}" SpecialFunc="None">
    <Declaration><![CDATA[FUNCTION_BLOCK FB_AxisSim
VAR_INPUT
    Enable : BOOL;
END_VAR
VAR_IN_OUT
    Axis : ST_Axis;
END_VAR
VAR
    tick : UDINT;
END_VAR
]]></Declaration>
    <Implementation>
      <ST><![CDATA[IF NOT Enable THEN
    Axis.State := E_AxisState.Idle;
    RETURN;
END_IF

tick := tick + 1;

// Ramp position + derive velocity so a subscription sees LREAL churn.
Axis.PositionMm       := Axis.PositionMm + 0.5;
Axis.VelocityMps      := 0.5 * SIN(UDINT_TO_LREAL(tick) * 0.05);
Axis.Accel            := LREAL_TO_REAL(0.1 * COS(UDINT_TO_LREAL(tick) * 0.05));
Axis.State            := E_AxisState.Moving;

// Motor block — keep values sane but moving.
Axis.Motor.Current    := 2.5 + LREAL_TO_REAL(0.5 * SIN(UDINT_TO_LREAL(tick) * 0.1));
Axis.Motor.Voltage    := 24.0;
Axis.Motor.Temperature:= 35.0 + 5.0 * SIN(UDINT_TO_LREAL(tick) * 0.01);
Axis.Motor.Rpm        := 1500 + LREAL_TO_DINT(200.0 * SIN(UDINT_TO_LREAL(tick) * 0.05));
Axis.Motor.Running    := TRUE;

// Encoder
Axis.Encoder.RawCounts      := Axis.Encoder.RawCounts + 12;
Axis.Encoder.PositionMm     := Axis.PositionMm;
Axis.Encoder.VelocityMmPerS := Axis.VelocityMps;
Axis.Encoder.Homed          := TRUE;

// Travel log — rolling window of last 8 positions.
Axis.TravelLog[1] := Axis.TravelLog[2];
Axis.TravelLog[2] := Axis.TravelLog[3];
Axis.TravelLog[3] := Axis.TravelLog[4];
Axis.TravelLog[4] := Axis.TravelLog[5];
Axis.TravelLog[5] := Axis.TravelLog[6];
Axis.TravelLog[6] := Axis.TravelLog[7];
Axis.TravelLog[7] := Axis.TravelLog[8];
Axis.TravelLog[8] := Axis.PositionMm;
]]></ST>
    </Implementation>
  </POU>
</TcPlcObject>
