From 2b41f3aa4b14b16d76f1447f3c76f42397842fb2 Mon Sep 17 00:00:00 2001 From: SCooper Date: Tue, 12 Apr 2022 11:14:11 +0100 Subject: [PATCH] Fixed an issue with setpoint generators not enabling when a limit is pressed --- solution/solution.tsproj | 2 + .../Applications/FB_SlitPairV2.TcPOU | 92 ++++++++++++++++++- solution/tc_project_app/tc_mca_std_lib | 2 +- 3 files changed, 90 insertions(+), 6 deletions(-) diff --git a/solution/solution.tsproj b/solution/solution.tsproj index 5f5121a..a534698 100644 --- a/solution/solution.tsproj +++ b/solution/solution.tsproj @@ -24,5 +24,7 @@ + + diff --git a/solution/tc_project_app/POUs/Application_Specific/Applications/FB_SlitPairV2.TcPOU b/solution/tc_project_app/POUs/Application_Specific/Applications/FB_SlitPairV2.TcPOU index 14cd000..0fb9817 100644 --- a/solution/tc_project_app/POUs/Application_Specific/Applications/FB_SlitPairV2.TcPOU +++ b/solution/tc_project_app/POUs/Application_Specific/Applications/FB_SlitPairV2.TcPOU @@ -49,6 +49,12 @@ VAR internalReset : BOOL; + //stupid internal MCPOWER blocks because Beckhoff won't let you enable a setpoint generator on a limit.... + mcPower_positiveAxis : MC_POWER; + mcPower_negativeAxis : MC_POWER; + mcPower_gapSizeAxis : MC_POWER; + mcPower_gapCentreAxis : MC_POWER; + END_VAR ]]> @@ -84,6 +90,10 @@ END_IF //Internal Reset linked to any axis reset internalReset := Reset OR GVL.astAxes[ID_positiveAxis].stControl.bReset OR GVL.astAxes[ID_negativeAxis].stControl.bReset OR GVL.astAxes[ID_gapSizeAxis].stControl.bReset OR GVL.astAxes[ID_gapCentreAxis].stControl.bReset; +IF GVL.astAxes[ID_positiveAxis].Axis.Status.Homing OR GVL.astAxes[ID_negativeAxis].Axis.Status.Homing OR GVL.astAxes[ID_gapSizeAxis].Axis.Status.Homing OR GVL.astAxes[ID_gapCentreAxis].Axis.Status.Homing THEN + iState:= E_SlitPairStatesV2.HOMING_INIT; +END_IF + //State machine (with error handling and reset logic) - WHAT ABOUT HOMING? IF Enable THEN CASE istate OF @@ -112,11 +122,24 @@ IF Enable THEN END_IF E_SlitPairStatesV2.ENABLE_REAL_GENERATORS: + //Hold MC_Power on until generators have enabled + mcPower_positiveAxis(Axis:=GVL.astAxes[ID_positiveAxis].Axis, Enable:=TRUE, Enable_Positive:=TRUE,Enable_Negative:=TRUE); + mcPower_negativeAxis(Axis:=GVL.astAxes[ID_negativeAxis].Axis, Enable:=TRUE, Enable_Positive:=TRUE,Enable_Negative:=TRUE); setpointGenEnable_positiveAxis.Execute:=TRUE; setpointGenEnable_negativeAxis.Execute:=TRUE; - iState:=E_SlitPairStatesV2.WAIT_FOR_VIRTUAL_MOVE_DONE; + iState:=E_SlitPairStatesV2.CHECK_REAL_AXIS_GENERATORS_ENABLED; + + E_SlitPairStatesV2.CHECK_REAL_AXIS_GENERATORS_ENABLED: + //Hold MC_Power on until generators have enabled + mcPower_positiveAxis(Axis:=GVL.astAxes[ID_positiveAxis].Axis, Enable:=TRUE, Enable_Positive:=TRUE,Enable_Negative:=TRUE); + mcPower_negativeAxis(Axis:=GVL.astAxes[ID_negativeAxis].Axis, Enable:=TRUE, Enable_Positive:=TRUE,Enable_Negative:=TRUE); + IF setPointGenEnable_positiveAxis.Enabled AND setpointGenEnable_negativeAxis.Enabled THEN + iState:= E_SlitPairStatesV2.WAIT_FOR_VIRTUAL_MOVE_DONE; + END_IF E_SlitPairStatesV2.WAIT_FOR_VIRTUAL_MOVE_DONE: + mcPower_positiveAxis(Axis:=GVL.astAxes[ID_positiveAxis].Axis, Enable:=TRUE, Enable_Positive:=TRUE,Enable_Negative:=TRUE); + mcPower_negativeAxis(Axis:=GVL.astAxes[ID_negativeAxis].Axis, Enable:=TRUE, Enable_Positive:=TRUE,Enable_Negative:=TRUE); IF NOT(GVL.astAxes[ID_gapSizeAxis].stStatus.bMoving OR GVL.astAxes[ID_gapCentreAxis].stStatus.bMoving) THEN iState:=E_SlitPairStatesV2.DISABLE_REAL_GENERATORS; END_IF @@ -127,9 +150,20 @@ IF Enable THEN iState:=E_SlitPairStatesV2.OPERATIONAL; E_SlitPairStatesV2.ENABLE_VIRTUAL_GENERATORS: + //Hold MC_Power on until generators have enabled + mcPower_gapSizeAxis(Axis:=GVL.astAxes[ID_gapSizeAxis].Axis, Enable:=TRUE, Enable_Positive:=TRUE,Enable_Negative:=TRUE); + mcPower_gapCentreAxis(Axis:=GVL.astAxes[ID_gapCentreAxis].Axis, Enable:=TRUE, Enable_Positive:=TRUE,Enable_Negative:=TRUE); setpointGenEnable_gapSizeAxis.Execute :=TRUE; setpointGenEnable_gapCentreAxis.Execute:=TRUE; - iState:=E_SlitPairStatesV2.WAIT_FOR_REAL_MOVE_DONE; + iState:=E_SlitPairStatesV2.CHECK_VIRTUAL_AXIS_GENERATORS_ENABLED; + + E_SlitPairStatesV2.CHECK_VIRTUAL_AXIS_GENERATORS_ENABLED: + //Hold MC_Power on until generators have enabled + mcPower_gapSizeAxis(Axis:=GVL.astAxes[ID_gapSizeAxis].Axis, Enable:=TRUE, Enable_Positive:=TRUE,Enable_Negative:=TRUE); + mcPower_gapCentreAxis(Axis:=GVL.astAxes[ID_gapCentreAxis].Axis, Enable:=TRUE, Enable_Positive:=TRUE,Enable_Negative:=TRUE); + IF setPointGenEnable_gapSizeAxis.Enabled AND setpointGenEnable_gapCentreAxis.Enabled THEN + iState:= E_SlitPairStatesV2.WAIT_FOR_REAL_MOVE_DONE; + END_IF E_SlitPairStatesV2.WAIT_FOR_REAL_MOVE_DONE: IF NOT(GVL.astAxes[1].stStatus.bMoving OR GVL.astAxes[2].stStatus.bMoving) THEN @@ -149,7 +183,25 @@ IF Enable THEN GVL.astAxes[ID_gapCentreAxis].stControl.bReset := TRUE; iState:= E_SlitPairStatesV2.INIT; END_IF - + + E_SlitPairStatesV2.HOMING_INIT: + GVL.astAxes[ID_positiveAxis].stControl.eCommand:= E_MotionFunctions.eHome; + GVL.astAxes[ID_positiveAxis].stControl.bExecute:= TRUE; + GVL.astAxes[ID_negativeAxis].stControl.eCommand:= E_MotionFunctions.eHome; + GVL.astAxes[ID_negativeAxis].stControl.bExecute:= TRUE; + iState:= E_SlitPairStatesV2.HOMING_CHECK; + + E_SlitPairStatesV2.HOMING_CHECK: + IF GVL.astAxes[ID_positiveAxis].Axis.Status.Homing OR GVL.astAxes[ID_negativeAxis].Axis.Status.Homing THEN + iState:= E_SlitPairStatesV2.HOMING_WAIT; + ELSE + iState:= E_SlitPairStatesV2.HOMING_INIT; + END_IF + + E_SlitPairStatesV2.HOMING_WAIT: + IF GVL.astAxes[ID_positiveAxis].Axis.Status.Homed AND GVL.astAxes[ID_negativeAxis].Axis.Status.Homed THEN + iState:= E_SlitPairStatesV2.INIT; + END_IF END_CASE END_IF @@ -298,6 +350,9 @@ fGapCentre := (GVL.astAxes[ID_positiveAxis].stStatus.fActPosition + GVL.astAxes[ + + + @@ -314,9 +369,27 @@ fGapCentre := (GVL.astAxes[ID_positiveAxis].stStatus.fActPosition + GVL.astAxes[ - + + + + + + + + + + + - + + + + + + + + + @@ -325,6 +398,15 @@ fGapCentre := (GVL.astAxes[ID_positiveAxis].stStatus.fActPosition + GVL.astAxes[ + + + + + + + + + diff --git a/solution/tc_project_app/tc_mca_std_lib b/solution/tc_project_app/tc_mca_std_lib index b480174..a0256fb 160000 --- a/solution/tc_project_app/tc_mca_std_lib +++ b/solution/tc_project_app/tc_mca_std_lib @@ -1 +1 @@ -Subproject commit b480174ad58ac9c260980ff5b8c8927f3d3d5cb9 +Subproject commit a0256fb69efb1012c6dd67e6f4a2e0eb1c3c16c9