Skip to content

Commit

Permalink
Merge branch 'develop'
Browse files Browse the repository at this point in the history
  • Loading branch information
@pswift066
pswift066 committed Mar 23, 2023
2 parents 20aa640 + db38e13 commit d14e60e
Show file tree
Hide file tree
Showing 3 changed files with 81 additions and 104 deletions.
42 changes: 22 additions & 20 deletions Base Project/Main/POUs/MAIN.TcPOU
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,26 @@
<Declaration><![CDATA[PROGRAM MAIN
VAR
// ========= System Control / FSM Variables =========
MainCommands : MainCommands_typ; // command struct for cycling thru the state machine
MainState : MainState_enum; // enumeration for the Main state machine
// ========= Linked NC Variables & References =========
Mover : ARRAY[0..GVL.NUM_MOVERS - 1] OF Mover; // instantiation of Mover objects. Matches the number of movers on the system
GroupRef : AXES_GROUP_REF; // linked to the Collision Avoidance group and passed cyclically to every Mover object
// ========= Objectives & Parameters =========
MoverList : MoverList;
ParameterSet : MotionParameters_typ;
Station : ARRAY[0..GVL.NUM_STATIONS - 1] OF Station;
PositionTrigger : ARRAY[0..3] OF PositionTrigger;
SpeedTrigger : ARRAY[0..1] OF SpeedTrigger;
Zone : ARRAY[0..3] OF Zone;
// ========= Example Application Variables =========
nextStation : USINT := 4;
Expand All @@ -13,32 +33,14 @@ VAR
highVelocity : LREAL := 1200; // mm/s
// ========= Helper, State, & Index Variables =========
i : UINT; // index variable; gets reused in various FOR loops
n : UINT; // secondary index variable, for enabling movers
allMoversEnabled : BOOL; // helper flag for verifying enable status of all movers
allMoversDisabled : BOOL; // helper flag for verifying disable status of all movers
allMoversHalted : BOOL; // helper flag for verifying halt status of all movers
StationTimer : ARRAY[0..GVL.NUM_STATIONS] OF TON; // timer blocks, for station dwells
MainState : MainState_enum; // enumeration for the Main state machine
MainCommands : MainCommands_typ; // command struct for cycling thru the state machine
// ========= Linked NC Variables & References =========
GroupRef : AXES_GROUP_REF; // linked to the Collision Avoidance group and passed cyclically to every Mover object
Mover : ARRAY[0..GVL.NUM_MOVERS - 1] OF Mover; // instantiation of Mover objects. Matches the number of movers on the system
// ========= Objectives & Parameters =========
MoverList : MoverList;
ParameterSet : MotionParameters_typ;
Station : ARRAY[0..GVL.NUM_STATIONS - 1] OF Station;
PositionTrigger : ARRAY[0..3] OF PositionTrigger;
SpeedTrigger : ARRAY[0..1] OF SpeedTrigger;
Zone : ARRAY[0..3] OF Zone;
StationTimer : ARRAY[0..GVL.NUM_STATIONS] OF TON; // timer blocks, for station dwells
// ========= XTS Utility Diagnostics =========
Expand Down
104 changes: 53 additions & 51 deletions Base Project/Main/POUs/XTS/MoverList.TcPOU
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,69 +44,71 @@ VAR_INPUT
END_VAR
VAR
positionSort : ARRAY[0..GVL.NUM_MOVERS - 1] OF POINTER TO Mover; // array copied from tracked movers, to be sorted by position
sortbuffer : ARRAY[0..GVL.NUM_MOVERS - 1] OF POINTER TO Mover; // temporary array, just to simplify sorting operation
currentLeast : POINTER TO Mover;
ringOffset : dint;
SortBuffer : ARRAY[0..GVL.NUM_MOVERS - 1] OF POINTER TO Mover; // initially empty array of movers, sorted in order by the position search
PopBuffer : ARRAY[0..GVL.NUM_MOVERS - 1] OF POINTER TO Mover; // temporary array, which simplifies the sorting operation at the minor cost of memory
currentLowDistance : LREAL;
currentMover : POINTER TO Mover;
directionalDistance : LREAL;
leastIDX : DINT;
i : DINT;
j : DINT;
END_VAR]]></Declaration>
<Implementation>
<ST><![CDATA[// copy tracked movers into temporary array
memcpy(ADR(sortbuffer), ADR(internalTrackedMovers), SIZEOF(internalTrackedMovers));
// sort temp array based on mover position (simple insertion sort)
FOR i := 0 TO SIZEOF(positionSort) / SIZEOF(positionSort[0]) - 1 DO
leastIDX := 0;
currentLeast := 0;
FOR j := 0 TO SIZEOF(sortBuffer) / SIZEOF(sortBuffer[0]) - 1 DO
IF currentLeast = 0 THEN
currentLeast := sortbuffer[j];
leastIDX := j;
ELSIF sortbuffer[j] <> 0 AND_THEN sortbuffer[j]^.AxisReference.NcToPlc.ModuloActPos < currentLeast^.AxisReference.NcToPlc.ModuloActPos THEN
currentLeast := sortbuffer[j];
leastIDX := j;
END_IF;
END_FOR
memcpy(ADR(positionSort[i]), ADR(currentLeast), SIZEOF(currentLeast));
memset(ADR(sortBuffer[leastIDX]), 0, SIZEOF(sortBuffer[0]));
END_FOR;
// determine offset where position exists
ringOffset := 0;
FOR i := 0 TO SIZEOF(positionSort) / SIZEOF(positionSort[0]) - 1 DO
IF positionSort[i] <> 0 AND_THEN positionSort[i]^.AxisReference.NcToPlc.ModuloActPos < Position THEN
IF Direction = MC_Positive_Direction THEN
ringOffset := i - Index;
ELSIF Direction = MC_Negative_Direction THEN
ringOffset := i + 1 + Index;
END_IF
END_IF
END_FOR
memcpy(ADR(PopBuffer), ADR(internalTrackedMovers), SIZEOF(internalTrackedMovers));
IF ringOffset < 0 THEN
ringOffset := ringOffset + THIS^.TrackedMoverCount;
ELSIF ringOffset >= THIS^.TrackedMoverCount THEN
ringOffset := ringOffset - THIS^.TrackedMoverCount;
END_IF
// count off either direction to return the proper mover
// For every entry in the sorted buffer...
FOR i := 0 TO SIZEOF( PopBuffer ) / SIZEOF( PopBuffer[0] ) - 1 DO
leastIDX := 0;
currentMover := 0;
currentLowDistance := 0;
// ... find the mover with the least directional distance to the input Position, including rollover
FOR j := 0 TO SIZEOF( PopBuffer ) / SIZEOF( PopBuffer[0] ) - 1 DO
IF Direction = MC_Positive_Direction THEN
// Calculate a directional distance from the input position to each mover, positive search
IF PopBuffer[j] <> 0 AND_THEN PopBuffer[j]^.AxisReference.NcToPlc.ModuloActPos <= Position THEN
directionalDistance := Position - PopBuffer[j]^.AxisReference.NcToPlc.ModuloActPos;
ELSIF PopBuffer[j] <> 0 AND_THEN PopBuffer[j]^.AxisReference.NcToPlc.ModuloActPos > Position THEN
directionalDistance := Position + ( GVL.TRACK_LENGTH - PopBuffer[j]^.AxisReference.NcToPlc.ModuloActPos );
END_IF
ELSIF Direction = MC_Negative_Direction THEN
// Calculate a directional distance from the input position to each mover, negative search
IF PopBuffer[j] <> 0 AND_THEN PopBuffer[j]^.AxisReference.NcToPlc.ModuloActPos <= Position THEN
directionalDistance := PopBuffer[j]^.AxisReference.NcToPlc.ModuloActPos + ( GVL.TRACK_LENGTH - Position );
ELSIF PopBuffer[j] <> 0 AND_THEN PopBuffer[j]^.AxisReference.NcToPlc.ModuloActPos > Position THEN
directionalDistance := PopBuffer[j]^.AxisReference.NcToPlc.ModuloActPos - Position;
END_IF
END_IF
// If the current mover is closer than the staged one, stage that instead
IF PopBuffer[j] <> 0 AND ( currentLowDistance = 0 OR directionalDistance < currentLowDistance ) THEN
currentMover := PopBuffer[j];
currentLowDistance := directionalDistance;
leastIDX := j;
END_IF
END_FOR
// ForLoop concludes with the next mover in currentMover slot. add it to the sorted buffer
memcpy( ADR( SortBuffer[i] ), ADR( currentMover ), SIZEOF( currentMover ) );
memset( ADR( PopBuffer[leastIDX] ), 0, SIZEOF( PopBuffer[0] ) );
END_FOR
// Output the proper indexed mover from the sorted list
IF Index < 0 OR Index >= THIS^.TrackedMoverCount THEN
GetMoverByLocation REF= 0;
ELSIF Direction = MC_Positive_Direction AND positionSort[ringOffset] <> 0 THEN
GetMoverByLocation REF= positionSort[ringOffset]^;
ELSIF Direction = MC_Negative_Direction AND positionSort[ringOffset] <> 0 THEN
GetMoverByLocation REF= positionSort[ringOffset]^;
GetMoverByLocation REF= 0;
ELSE
GetMoverByLocation REF= 0;
GetMoverByLocation REF= SortBuffer[Index]^;
END_IF
// If the MoverList has no proper mover to return, then return an imposter ErrorMover, whose method calls all trigger an error message
IF __ISVALIDREF(GetMoverByLocation) = FALSE THEN
ErrorMover.SourceInstancePath := THIS^.InstancePath;
Expand Down
39 changes: 6 additions & 33 deletions Base Project/Main/POUs/XTS/Objective.TcPOU
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,12 @@ END_VAR]]></Declaration>
<Declaration><![CDATA[VAR
END_VAR]]></Declaration>
<Implementation>
<ST><![CDATA[]]></ST>
<ST><![CDATA[
// If no real mover can be returned as Current, return an imposter instead to output error messages
IF __ISVALIDREF( CurrentMover ) = FALSE THEN
ErrorMover.SourceInstancePath := THIS^.InstancePath;
CurrentMover REF= ErrorMover;
END_IF]]></ST>
</Implementation>
</Get>
</Property>
Expand Down Expand Up @@ -196,37 +201,5 @@ ELSE
END_IF]]></ST>
</Implementation>
</Method>
<LineIds Name="Objective">
<LineId Id="2" Count="0" />
</LineIds>
<LineIds Name="Objective.CurrentMover.Get">
<LineId Id="2" Count="0" />
</LineIds>
<LineIds Name="Objective.RegisterMover">
<LineId Id="3" Count="29" />
<LineId Id="2" Count="0" />
</LineIds>
<LineIds Name="Objective.RegisterMoverList">
<LineId Id="3" Count="12" />
<LineId Id="2" Count="0" />
</LineIds>
<LineIds Name="Objective.TrackedMoverCount.Get">
<LineId Id="3" Count="7" />
<LineId Id="2" Count="0" />
</LineIds>
<LineIds Name="Objective.TrackedMovers.Get">
<LineId Id="2" Count="0" />
</LineIds>
<LineIds Name="Objective.UnregisterAll">
<LineId Id="3" Count="0" />
<LineId Id="2" Count="0" />
</LineIds>
<LineIds Name="Objective.UnregisterCurrent">
<LineId Id="2" Count="0" />
</LineIds>
<LineIds Name="Objective.UnregisterMover">
<LineId Id="3" Count="24" />
<LineId Id="2" Count="0" />
</LineIds>
</POU>
</TcPlcObject>

0 comments on commit d14e60e

Please sign in to comment.