From 0b67b83b6d9a0e1dac77c91fd710d8bf396912b9 Mon Sep 17 00:00:00 2001 From: Peter Swift Date: Wed, 22 Mar 2023 15:55:07 -0500 Subject: [PATCH 1/3] Refactored GetMoverByLocation. Needs testing. --- Base Project/Main/POUs/XTS/MoverList.TcPOU | 104 +++++++++++---------- 1 file changed, 53 insertions(+), 51 deletions(-) diff --git a/Base Project/Main/POUs/XTS/MoverList.TcPOU b/Base Project/Main/POUs/XTS/MoverList.TcPOU index d732b4d..d78e554 100644 --- a/Base Project/Main/POUs/XTS/MoverList.TcPOU +++ b/Base Project/Main/POUs/XTS/MoverList.TcPOU @@ -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]]> 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]^.AxisReference.NcToPlc.ModuloActPos <= Position THEN + directionalDistance := Position - PopBuffer[j]^.AxisReference.NcToPlc.ModuloActPos; + ELSIF 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]^.AxisReference.NcToPlc.ModuloActPos <= Position THEN + directionalDistance := PopBuffer[j]^.AxisReference.NcToPlc.ModuloActPos + ( GVL.TRACK_LENGTH - Position ); + ELSIF 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 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; From 7eef4b9ceaa4b879743bc8bcd1e8d8ef4e6201ff Mon Sep 17 00:00:00 2001 From: Peter Swift Date: Thu, 23 Mar 2023 12:03:22 -0500 Subject: [PATCH 2/3] Minor update to refactor of GetMoverByLocation. Resolves selection issue with previous commit. Tested, appears correct --- Base Project/Main/POUs/XTS/MoverList.TcPOU | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/Base Project/Main/POUs/XTS/MoverList.TcPOU b/Base Project/Main/POUs/XTS/MoverList.TcPOU index d78e554..e8718f4 100644 --- a/Base Project/Main/POUs/XTS/MoverList.TcPOU +++ b/Base Project/Main/POUs/XTS/MoverList.TcPOU @@ -72,23 +72,23 @@ FOR i := 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]^.AxisReference.NcToPlc.ModuloActPos <= Position THEN + IF PopBuffer[j] <> 0 AND_THEN PopBuffer[j]^.AxisReference.NcToPlc.ModuloActPos <= Position THEN directionalDistance := Position - PopBuffer[j]^.AxisReference.NcToPlc.ModuloActPos; - ELSIF PopBuffer[j]^.AxisReference.NcToPlc.ModuloActPos > Position THEN + 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]^.AxisReference.NcToPlc.ModuloActPos <= Position THEN + 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]^.AxisReference.NcToPlc.ModuloActPos > Position THEN + 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 currentLowDistance = 0 OR directionalDistance < currentLowDistance THEN + IF PopBuffer[j] <> 0 AND ( currentLowDistance = 0 OR directionalDistance < currentLowDistance ) THEN currentMover := PopBuffer[j]; currentLowDistance := directionalDistance; leastIDX := j; @@ -105,7 +105,7 @@ END_FOR IF Index < 0 OR Index >= THIS^.TrackedMoverCount THEN GetMoverByLocation REF= 0; ELSE - GetMoverByLocation REF= SortBuffer[Index]; + GetMoverByLocation REF= SortBuffer[Index]^; END_IF From 4d1b1cdfa1dff13ec21a44a563e2063537aefc50 Mon Sep 17 00:00:00 2001 From: Peter Swift Date: Thu, 23 Mar 2023 12:45:20 -0500 Subject: [PATCH 3/3] Added an Error Mover imposter clause to the abstract objective class. This doesn't help with Mover.MasterMover, but that isn't currently possible due to data recursion. --- Base Project/Main/POUs/MAIN.TcPOU | 42 +++++++++++----------- Base Project/Main/POUs/XTS/Objective.TcPOU | 39 ++++---------------- 2 files changed, 28 insertions(+), 53 deletions(-) diff --git a/Base Project/Main/POUs/MAIN.TcPOU b/Base Project/Main/POUs/MAIN.TcPOU index 62989ae..47c72b8 100644 --- a/Base Project/Main/POUs/MAIN.TcPOU +++ b/Base Project/Main/POUs/MAIN.TcPOU @@ -4,6 +4,26 @@ - + @@ -196,37 +201,5 @@ ELSE END_IF]]> - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file