Skip to content

Commit

Permalink
Change example ApplicationMover method close #84
Browse files Browse the repository at this point in the history
  • Loading branch information
netbymatt committed Dec 31, 2024
1 parent d641beb commit f969a12
Show file tree
Hide file tree
Showing 3 changed files with 120 additions and 115 deletions.
2 changes: 1 addition & 1 deletion Base Project/Base Project.tsproj
Original file line number Diff line number Diff line change
Expand Up @@ -32098,7 +32098,7 @@ External Setpoint Generation:
</Motion>
<Plc>
<Project GUID="{DD3A0B29-8E7C-4E32-989D-B0222C998777}" Name="PLC1" PrjFilePath="PLC1\PLC1.plcproj" TmcFilePath="PLC1\PLC1.tmc" ReloadTmc="true" AmsPort="851" FileArchiveSettings="#x000e" SymbolicMapping="true">
<Instance Id="#x08502000" TcSmClass="TComPlcObjDef" KeepUnrestoredLinks="2" TmcHash="{EF401F15-6079-0CBF-97AC-9E4A6BA8DF55}" TmcPath="PLC1\PLC1.tmc">
<Instance Id="#x08502000" TcSmClass="TComPlcObjDef" KeepUnrestoredLinks="2" TmcHash="{CB777EBB-E8AE-7D49-6680-A045ABEDD19E}" TmcPath="PLC1\PLC1.tmc">
<Name>PLC1 Instance</Name>
<CLSID ClassFactory="TcPlc30">{08500001-0000-0000-F000-000000000064}</CLSID>
<Vars VarGrpType="2" AreaNo="1">
Expand Down
92 changes: 54 additions & 38 deletions Base Project/PLC1/POUs/ApplicationMover.TcPOU
Original file line number Diff line number Diff line change
Expand Up @@ -8,9 +8,7 @@ END_VAR
VAR_OUTPUT
END_VAR
VAR
cnt: UDINT;
_PositionLag: LREAL;
fbMC_CalcDynamicsByRampTime: MC_CalcDynamicsByRampTime;
END_VAR
]]></Declaration>
<Implementation>
Expand All @@ -31,58 +29,76 @@ SUPER^.Cyclic(GroupReference);
// you must call Mover[] array index directly, as Station/Zone/PositionTrig/SpeedTrig do not know about ApplicationMover add ons
// Add ApplicationMover specific code here
cnt:= cnt + 1;
_PositionLag:= THIS^.AxisReference.NcToPlc.PosDiff;
// example of populating a local position lag variable to be used with the position lag parameter
_PositionLag:= THIS^.AxisReference.NcToPlc.PosDiff;
// Latch Errors
IF fbMC_CalcDynamicsByRampTime.Error THEN
ErrorID := fbMC_CalcDynamicsByRampTime.ErrorID;
ErrorOrigin := CONCAT( InstancePath, '.fbMC_CalcDynamicsByRampTime' );
//LocalVars.State := MV_ERROR;
END_IF
// an example error response is commented below
// IF fbMC_Example.Error THEN
// ErrorID := fbMC_Example.ErrorID;
// ErrorOrigin := CONCAT( InstancePath, '.fbMC_Example' );
// // if error requires mover to be reset sendto the error state
// LocalVars.State := MV_ERROR;
// END_IF
// Call Fbs Cyclically
fbMC_CalcDynamicsByRampTime();
// if FBs added to the mover need to be called cyclically add them here
]]></ST>
</Implementation>
</Method>
<Method Name="MoveToStationByRampTime" Id="{30fb390b-b4a8-0444-29f0-600dae10b753}">
<Declaration><![CDATA[METHOD MoveToStationByRampTime
<Method Name="IsInWindow" Id="{824790c2-8eaf-0f47-07fd-44b548cd851d}">
<Declaration><![CDATA[METHOD IsInWindow : BOOL
VAR_INPUT
DestinationStation : REFERENCE TO Station;
Velocity : LREAL;
RampTime : LREAL;
Stiffness : LREAL;
Gap : LREAL;
Position : LREAL;
Tolerance : LREAL;
Positive : BOOL := TRUE;
Negative : BOOL := TRUE;
END_VAR
VAR
MotionParameters: MotionParameters_typ;
END_VAR]]></Declaration>
LowLimit : LREAL;
HighLimit : LREAL;
TrackLength : LREAL;
CurrentPosition: LREAL;
END_VAR
]]></Declaration>
<Implementation>
<ST><![CDATA[// This method internally calls MC_CalcDynamicsByRampTime from Tc2_MC2.lib
// https://infosys.beckhoff.com/content/1033/tcplclib_tc2_mc2/13611300875.html?id=1298283191705576852
<ST><![CDATA[// return true if the mover is in a window around the provided position, within the provided tolerance (inclusive)
// rollover at the 0-position is handled automatically
// the window can be made positive or negative only with the Positive or Negative options
// default: true when Position - Tolerance <= Mover Position <= Position + Tolerance
// positive = true: true when Position <= Mover Position <= Position + Tolerance
// negative = true: true when Position - Tolerance <= Mover Position <= Position
fbMC_CalcDynamicsByRampTime(
Velocity:= Velocity,
RampTime:= RampTime,
Stiffness:= Stiffness,
Error=> ,
ErrorID=> ,
Acceleration=> ,
Deceleration=> ,
Jerk=> );
// default condition
IsInWindow := FALSE;
MotionParameters.Velocity := Velocity;
MotionParameters.Acceleration := fbMC_CalcDynamicsByRampTime.Acceleration;
MotionParameters.Deceleration := fbMC_CalcDynamicsByRampTime.Deceleration;
MotionParameters.Jerk := fbMC_CalcDynamicsByRampTime.Jerk;
MotionParameters.Gap := Gap;
MotionParameters.Direction := Tc3_Mc3Definitions.MC_Direction.mcDirectionPositive;
// if Positive and Negative are false, then the mover is not in the window
// if the tolerance is negative or zero it will return false
IF (Positive = FALSE AND Negative = FALSE) THEN
RETURN;
END_IF
// for simplicity in calculating modulo calculate positions as 1 full track length higher than reported
// store the track length for easy reference
TrackLength := LocalVars.Track^.Length;
// calculate low limit
LowLimit := Position + TrackLength;
IF (Negative) THEN
LowLimit := LowLimit - Tolerance;
END_IF
// calculate high limit
HighLimit := Position + TrackLength;
IF (Positive) THEN
HighLimit := HighLimit + Tolerance;
END_IF
SUPER^.MotionParameters:= MotionParameters;
SUPER^.MoveToStation(DestinationStation);]]></ST>
// test limits
CurrentPosition := (TrackInfo.TrackPosition + TrackLength);
IsInWindow := (LowLimit <= CurrentPosition) AND (CurrentPosition <= HighLimit);]]></ST>
</Implementation>
</Method>
<Property Name="PositionLag" Id="{2f9b4da0-9f46-0501-2862-3b56ba7816bb}">
Expand Down
141 changes: 65 additions & 76 deletions Base Project/PLC1/POUs/MAIN.TcPOU
Original file line number Diff line number Diff line change
Expand Up @@ -19,12 +19,12 @@ VAR
highVelocity : LREAL := 1200; // mm/s
StationTimer : ARRAY[0..Param.NUM_STATIONS] OF TON; // timer blocks, for station dwells
InitialParameterSet : MotionParameters_typ;
RecoveryComplete : BOOL;
mcDirectionPositive : Tc2_MC2.MC_Direction;
Trig0MoverIndex : UDINT;
Trig0MoverPosLag : LREAL;
MoverInWindow: BOOL;
END_VAR
]]></Declaration>
<Implementation>
Expand Down Expand Up @@ -218,85 +218,74 @@ nextStation := 4;]]></ST>
</Action>
<Action Name="StationLogic" Id="{1adacb9d-add1-0f3c-1a94-78c322ce5f05}">
<Implementation>
<ST><![CDATA[ (*Mover routing logic should be written here to define the system's operating behavior.
Station implementations, Position Trigger logic, etc. In general, you can remove the example code
here down to the short line break that looks like this: // ============= *)
<ST><![CDATA[(*Mover routing logic should be written here to define the system's operating behavior.
Station implementations, Position Trigger logic, etc. In general, you can remove the example code
here down to the short line break that looks like this: // ============= *)
// Station 0 Logic
IF XTS.Station[0].MoverInPosition THEN
StationTimer[0](IN := TRUE, PT := T#1000MS);
IF StationTimer[0].Q THEN
XTS.Station[0].CurrentMover.SetVelocity(mediumVelocity).MoveToStation(XTS.Station[1]);
END_IF
ELSE
StationTimer[0](IN := FALSE);
END_IF
// Station 1 Logic
IF XTS.Station[1].MoverInPosition THEN
StationTimer[1](IN := TRUE, PT := T#250MS);
IF StationTimer[1].Q THEN
XTS.Station[1].CurrentMover.SetVelocity(mediumVelocity).MoveToStation(XTS.Station[nextStation]);
nextStation := nextStation - 1;
IF nextStation = 1 THEN
nextStation := 4;
END_IF
END_IF
ELSE
StationTimer[1](IN := FALSE);
END_IF
// Group of Stations 2, 3, and 4 Logic
IF XTS.Station[2].MoverInPosition AND XTS.Station[3].MoverInPosition AND XTS.Station[4].MoverInPosition THEN
StationTimer[2](IN := TRUE, PT := T#750MS);
IF StationTimer[2].Q THEN
StationTimer[2](IN := FALSE);
XTS.Station[2].CurrentMover.MoveVelocity(mediumVelocity);
XTS.Station[3].CurrentMover.MoveVelocity(mediumVelocity);
XTS.Station[4].CurrentMover.MoveVelocity(mediumVelocity);
END_IF
END_IF
// Position Trigger 0 Logic
IF XTS.PositionTrigger[0].MoverPassedPosition THEN
XTS.PositionTrigger[0].CurrentMover.SetVelocity(lowVelocity);
// Station 0 Logic
IF XTS.Station[0].MoverInPosition THEN
StationTimer[0](IN := TRUE, PT := T#1000MS);
IF StationTimer[0].Q THEN
XTS.Mover[XTS.Station[0].CurrentMover.MoverIndex].MoveToStationByRampTime(
DestinationStation:= XTS.Station[1],
Velocity:= 500,
RampTime:= 1.5,
Stiffness:= 0.5,
Gap:= 85);
END_IF
ELSE
StationTimer[0](IN := FALSE);
END_IF
// Station 1 Logic
IF XTS.Station[1].MoverInPosition THEN
StationTimer[1](IN := TRUE, PT := T#250MS);
IF StationTimer[1].Q THEN
XTS.Station[1].CurrentMover.MoveToStation(XTS.Station[nextStation]);
nextStation := nextStation - 1;
IF nextStation = 1 THEN
nextStation := 4;
END_IF
END_IF
ELSE
StationTimer[1](IN := FALSE);
END_IF
// Group of Stations 2, 3, and 4 Logic
IF XTS.Station[2].MoverInPosition AND XTS.Station[3].MoverInPosition AND XTS.Station[4].MoverInPosition THEN
StationTimer[2](IN := TRUE, PT := T#750MS);
IF StationTimer[2].Q THEN
StationTimer[2](IN := FALSE);
// To Call ApplicationMover addon Method/Properties/Variables, you must call Mover[] array index directly
// Get MoverIndex for later use with ApplicationMover addons
Trig0MoverIndex := XTS.PositionTrigger[0].CurrentMover.MoverIndex;
// Call ApplicationMover Method/Properties/Variables using previously latched MoverIndex
Trig0MoverPosLag := XTS.Mover[Trig0MoverIndex].PositionLag;
XTS.PositionTrigger[0].MuteCurrent();
XTS.Station[2].CurrentMover.MotionParameters:= XTS.InitialParameterSet;
XTS.Station[3].CurrentMover.MotionParameters:= XTS.InitialParameterSet;
XTS.Station[4].CurrentMover.MotionParameters:= XTS.InitialParameterSet;
END_IF
XTS.Station[2].CurrentMover.MoveVelocity(500);
XTS.Station[3].CurrentMover.MoveVelocity(500);
XTS.Station[4].CurrentMover.MoveVelocity(500);
END_IF
END_IF
// Position Trigger 0 Logic
IF XTS.PositionTrigger[0].MoverPassedPosition THEN
XTS.PositionTrigger[0].CurrentMover.SetVelocity(lowVelocity);
// To Call ApplicationMover addon Method/Properties/Variables, you must call Mover[] array index directly
// Get MoverIndex for later use with ApplicationMover addons
Trig0MoverIndex := XTS.PositionTrigger[0].CurrentMover.MoverIndex;
// Call ApplicationMover Method/Properties/Variables using previously latched MoverIndex
Trig0MoverPosLag := XTS.Mover[Trig0MoverIndex].PositionLag;
XTS.PositionTrigger[0].MuteCurrent();
END_IF
// Position Trigger 1 Logic
IF XTS.PositionTrigger[1].MoverPassedPosition THEN
XTS.PositionTrigger[1].CurrentMover.SetAcceleration(5E2);
XTS.PositionTrigger[1].CurrentMover.SetVelocity(highVelocity);
XTS.PositionTrigger[1].MuteCurrent();
END_IF
// Position Trigger 1 Logic
IF XTS.PositionTrigger[1].MoverPassedPosition THEN
XTS.PositionTrigger[1].CurrentMover.SetAcceleration(5E2);
XTS.PositionTrigger[1].CurrentMover.SetVelocity(highVelocity);
XTS.PositionTrigger[1].MuteCurrent();
END_IF
// Position Trigger 2 Logic
IF XTS.PositionTrigger[2].MoverPassedPosition THEN
XTS.PositionTrigger[2].CurrentMover.SetAcceleration(1E4);
XTS.PositionTrigger[2].CurrentMover.SetVelocity(lowVelocity);
XTS.PositionTrigger[2].CurrentMover.MoveToStation(XTS.Station[0]);
XTS.PositionTrigger[2].MuteCurrent();
END_IF]]></ST>
// Position Trigger 2 Logic
IF XTS.PositionTrigger[2].MoverPassedPosition THEN
XTS.PositionTrigger[2].CurrentMover.SetAcceleration(1E4);
XTS.PositionTrigger[2].CurrentMover.SetVelocity(lowVelocity);
XTS.PositionTrigger[2].CurrentMover.MoveToStation(XTS.Station[0]);
XTS.PositionTrigger[2].MuteCurrent();
END_IF]]></ST>
</Implementation>
</Action>
</POU>
Expand Down

0 comments on commit f969a12

Please sign in to comment.