383 lines
8.6 KiB
XML
383 lines
8.6 KiB
XML
<?xml version="1.0" encoding="utf-8"?>
|
|
<TcPlcObject Version="1.1.0.1">
|
|
<POU Name="FB_Aligner" Id="{5be6cab7-2294-48e5-869c-33773329143e}" SpecialFunc="None">
|
|
<Declaration><![CDATA[FUNCTION_BLOCK FB_Aligner EXTENDS FB_PackMLGeneric
|
|
VAR_INPUT
|
|
xEnableVacuum : BOOL;
|
|
xExecute : BOOL;
|
|
xReleaseAlarms : BOOL;
|
|
xConfirmAlarms : BOOL;
|
|
END_VAR
|
|
VAR_OUTPUT
|
|
xVacuumEnabled : BOOL;
|
|
END_VAR
|
|
VAR_IN_OUT
|
|
stIO : ST_Aligner_IO;
|
|
stHMIInterface : ST_HMI_Aligner;
|
|
END_VAR
|
|
VAR
|
|
_fbValveVacuum : FB_Valve('Vacuum');
|
|
_stValveVacuumConfig : ST_ValveConfig;
|
|
|
|
_fbXAxis : FB_AxisPTP;
|
|
_fbYAxis : FB_AxisPTP;
|
|
|
|
_rtExecute : R_TRIG;
|
|
_xStartAlign : BOOL;
|
|
END_VAR
|
|
]]></Declaration>
|
|
<Implementation>
|
|
<ST><![CDATA[_fbXAxis(
|
|
xInvertCalibrationCam:= FALSE,
|
|
xEnablePositive:= TRUE,
|
|
xEnableNegative:= TRUE,
|
|
rOverride:= 100.0,
|
|
lrVelocity:= 2.5,
|
|
lrAcceleration:= 0,
|
|
lrDecelleration:= 0,
|
|
lrJerk:= 0,
|
|
xConfirmAlarms:= xConfirmAlarms,
|
|
stIO := stIO.stAxisX);
|
|
|
|
_fbYAxis(
|
|
xInvertCalibrationCam:= FALSE,
|
|
xEnablePositive:= TRUE,
|
|
xEnableNegative:= TRUE,
|
|
rOverride:= 100.0,
|
|
lrVelocity:= 2.5,
|
|
lrAcceleration:= 0,
|
|
lrDecelleration:= 0,
|
|
lrJerk:= 0,
|
|
xConfirmAlarms:= xConfirmAlarms,
|
|
stIO := stIO.stAxisY);
|
|
|
|
_fbValveVacuum(
|
|
xAutomaticOpen:= xEnableVacuum,
|
|
xReleaseErrors:= xReleaseAlarms,
|
|
stValveConfig:= _stValveVacuumConfig,
|
|
xReleaseManualMode:= _xReleaseManualMode,
|
|
xConfirmAlarms:= xConfirmAlarms,
|
|
stHMIInterface:= stHMIInterface.stVacuumValve,
|
|
stIO := stIO.stValveVacuum);
|
|
|
|
|
|
// =============================
|
|
// Call isa88 base state machine
|
|
// =============================
|
|
SUPER^(stPackMLHMIInterface := stHMIInterface.stStationCmds);
|
|
|
|
_rtExecute(CLK:= xExecute, Q=> _xStartAlign);
|
|
|
|
// Handle enable disable vacuum command
|
|
xVacuumEnabled := stIO.i_xVacuumOk;]]></ST>
|
|
</Implementation>
|
|
<Method Name="FB_init" Id="{5048e086-25a4-4fbb-bf29-7a016bcf2277}">
|
|
<Declaration><![CDATA[//FB_Init is always available implicitly and it is used primarily for initialization.
|
|
//The return value is not evaluated. For a specific influence, you can also declare the
|
|
//methods explicitly and provide additional code there with the standard initialization
|
|
//code. You can evaluate the return value.
|
|
METHOD FB_Init: BOOL
|
|
VAR_INPUT
|
|
bInitRetains: BOOL; // TRUE: the retain variables are initialized (reset warm / reset cold)
|
|
bInCopyCode: BOOL; // TRUE: the instance will be copied to the copy code afterward (online change)
|
|
END_VAR
|
|
]]></Declaration>
|
|
<Implementation>
|
|
<ST><![CDATA[_stSMConfig.xClearingDisabled := TRUE;
|
|
_stSMConfig.xStartingDisabled := TRUE;
|
|
_stSMConfig.xHeldDisabled := TRUE;
|
|
_stSMConfig.xSuspededDisabled := TRUE;
|
|
_stSMConfig.xCompletedDisabled := FALSE;
|
|
|
|
// Valve config
|
|
_stValveVacuumConfig.xHasOpenFeedback := FALSE;
|
|
_stValveVacuumConfig.xHasClosedFeedback := FALSE;]]></ST>
|
|
</Implementation>
|
|
</Method>
|
|
<Method Name="M_Clearing" Id="{0b5b5108-3886-40a3-876f-a3460b5d5089}">
|
|
<Declaration><![CDATA[METHOD PROTECTED M_Clearing
|
|
]]></Declaration>
|
|
<Implementation>
|
|
<ST><![CDATA[CASE _iSSM OF
|
|
0:
|
|
// TODO: Reset axes
|
|
M_StateComplete();
|
|
END_CASE]]></ST>
|
|
</Implementation>
|
|
</Method>
|
|
<Method Name="M_Execute" Id="{d508050c-8f62-4fb2-be6c-88cfee1bab3b}">
|
|
<Declaration><![CDATA[METHOD PROTECTED M_Execute
|
|
VAR_INST
|
|
_deltaX : REAL;
|
|
_deltaY : REAL;
|
|
|
|
_uiRuns : UINT;
|
|
_tonWait : TON;
|
|
|
|
_xAxisXMoved : BOOL;
|
|
_xAxisYMoved : BOOL;
|
|
END_VAR]]></Declaration>
|
|
<Implementation>
|
|
<ST><![CDATA[CASE _iSSM OF
|
|
// Calculate delta to move
|
|
0:
|
|
// Middle of camera is X = 0 AND Y = 37.5
|
|
_deltaX := DINT_TO_REAL(stIO.i_diXOffset) * -0.001;
|
|
_deltaY := ((DINT_TO_REAL(stIO.i_diYOffset) * 0.001) - 37.5) * -1;
|
|
|
|
IF (ABS(_deltaX) < 10.0) AND (ABS(_deltaY) < 10.0) AND (NOT _fbXAxis.xError) AND (NOT _fbYAxis.xError) THEN
|
|
_xAxisXMoved := FALSE;
|
|
_xAxisYMoved := FALSE;
|
|
_iSSM := 10;
|
|
END_IF
|
|
|
|
// Start axis movement
|
|
5:
|
|
// Start X-Axis movement
|
|
IF (NOT _xAxisXMoved) THEN
|
|
CASE _fbXAxis.M_MoveRel(lrRelDist := _deltaX) OF
|
|
E_CmdResult.ACCEPTED:
|
|
_xAxisXMoved := TRUE;
|
|
|
|
E_CmdResult.BUSY:
|
|
;
|
|
|
|
E_CmdResult.REJECTED,
|
|
E_CmdResult.ERROR:
|
|
M_CmdAbort();
|
|
END_CASE
|
|
END_IF
|
|
|
|
// Start Y-Axis movement
|
|
IF (NOT _xAxisYMoved) THEN
|
|
CASE _fbYAxis.M_MoveRel(lrRelDist := _deltaY) OF
|
|
E_CmdResult.ACCEPTED:
|
|
_xAxisYMoved := TRUE;
|
|
|
|
E_CmdResult.BUSY:
|
|
;
|
|
|
|
E_CmdResult.REJECTED,
|
|
E_CmdResult.ERROR:
|
|
M_CmdAbort();
|
|
END_CASE
|
|
END_IF
|
|
|
|
// Wait for all axes to be started
|
|
IF _xAxisXMoved AND _xAxisYMoved THEN
|
|
_iSSM := 10;
|
|
END_IF
|
|
|
|
// Wait for axis movements to be done
|
|
10:
|
|
IF _fbXAxis.xDone AND _fbYAxis.xDone THEN
|
|
_uiRuns := _uiRuns + 1;
|
|
|
|
// Do alignment twice because of backlash in aligner station
|
|
IF _uiRuns > 1 THEN
|
|
_iSSM := 30;
|
|
ELSE
|
|
_iSSM := 20;
|
|
END_IF
|
|
END_IF
|
|
|
|
// Wait some time to stabelize reading
|
|
20:
|
|
_tonWait(IN := TRUE, PT := T#2S);
|
|
IF _tonWait.Q THEN
|
|
_tonWait(IN := FALSE);
|
|
_iSSM := 0;
|
|
END_IF
|
|
|
|
30:
|
|
_uiRuns := 0;
|
|
_eCmd := E_PackMLCmd.COMPLETE;
|
|
END_CASE]]></ST>
|
|
</Implementation>
|
|
</Method>
|
|
<Method Name="M_Resetting" Id="{015c2bf3-1e45-46dd-93fe-a1f154bfd906}">
|
|
<Declaration><![CDATA[METHOD PROTECTED M_Resetting
|
|
VAR_INST
|
|
_xAxisXEnabled : BOOL;
|
|
_xAxisYEnabled : BOOL;
|
|
|
|
_xAxisXHomed : BOOL;
|
|
_xAxisYHomed : BOOL;
|
|
|
|
_xAxisXMoved : BOOL;
|
|
_xAxisYMoved : BOOL;
|
|
END_VAR]]></Declaration>
|
|
<Implementation>
|
|
<ST><![CDATA[CASE _iSSM OF
|
|
// Reset axis flags
|
|
0:
|
|
_xAxisXEnabled := FALSE;
|
|
_xAxisYEnabled := FALSE;
|
|
_xAxisXHomed := FALSE;
|
|
_xAxisYHomed := FALSE;
|
|
_xAxisXMoved := FALSE;
|
|
_xAxisYMoved := FALSE;
|
|
|
|
_iSSM := 5;
|
|
|
|
// Enable axes
|
|
5:
|
|
// Enable X-Axis
|
|
IF (NOT _xAxisXEnabled) THEN
|
|
CASE _fbXAxis.M_Enable() OF
|
|
E_CmdResult.ACCEPTED:
|
|
_xAxisXEnabled := TRUE;
|
|
|
|
E_CmdResult.BUSY:
|
|
;
|
|
|
|
E_CmdResult.REJECTED,
|
|
E_CmdResult.ERROR:
|
|
M_CmdAbort();
|
|
END_CASE
|
|
END_IF
|
|
|
|
// Enable Y-Axis
|
|
IF (NOT _xAxisYEnabled) THEN
|
|
CASE _fbYAxis.M_Enable() OF
|
|
E_CmdResult.ACCEPTED:
|
|
_xAxisYEnabled := TRUE;
|
|
|
|
E_CmdResult.BUSY:
|
|
;
|
|
|
|
E_CmdResult.REJECTED,
|
|
E_CmdResult.ERROR:
|
|
M_CmdAbort();
|
|
END_CASE
|
|
END_IF
|
|
|
|
// Check if both could be enabled
|
|
IF _xAxisXEnabled AND _xAxisYEnabled THEN
|
|
_iSSM := 10;
|
|
END_IF
|
|
|
|
|
|
// Wait for axes to be enabled
|
|
6:
|
|
IF _fbXAxis.xDone AND _fbYAxis.xDone THEN
|
|
_iSSM := 10;
|
|
END_IF
|
|
|
|
IF _fbXAxis.xError OR _fbYAxis.xError THEN
|
|
M_CmdAbort();
|
|
END_IF
|
|
|
|
// Check axis homed state
|
|
7:
|
|
IF _fbXAxis.xHomed THEN
|
|
_xAxisXHomed := TRUE;
|
|
END_IF
|
|
|
|
IF _fbYAxis.xHomed THEN
|
|
_xAxisYHomed := TRUE;
|
|
END_IF
|
|
|
|
_iSSM := 10;
|
|
|
|
// Check if both axes are homed
|
|
10:
|
|
// Home X-Axis
|
|
IF (NOT _xAxisXHomed) AND (NOT _fbXAxis.xHomed) THEN
|
|
CASE _fbXAxis.M_Homing() OF
|
|
E_CmdResult.ACCEPTED:
|
|
_xAxisXHomed := TRUE;
|
|
|
|
E_CmdResult.BUSY:
|
|
;
|
|
|
|
E_CmdResult.REJECTED,
|
|
E_CmdResult.ERROR:
|
|
M_CmdAbort();
|
|
END_CASE
|
|
END_IF
|
|
|
|
// Home Y-Axis
|
|
IF (NOT _xAxisYHomed) AND (NOT _fbYAxis.xHomed) THEN
|
|
CASE _fbYAxis.M_Homing() OF
|
|
E_CmdResult.ACCEPTED:
|
|
_xAxisYHomed := TRUE;
|
|
|
|
E_CmdResult.BUSY:
|
|
;
|
|
|
|
E_CmdResult.REJECTED,
|
|
E_CmdResult.ERROR:
|
|
M_CmdAbort();
|
|
END_CASE
|
|
END_IF
|
|
|
|
IF _xAxisYHomed AND _xAxisYHomed THEN
|
|
// Goto wait for homing state
|
|
_iSSM := 15;
|
|
ELSIF _fbXAxis.xHomed AND _fbYAxis.xHomed THEN
|
|
// Goto move to 0 state
|
|
_iSSM := 20;
|
|
END_IF
|
|
|
|
// Wait for homing to be done
|
|
15:
|
|
IF (_fbXAxis.xDone OR _fbXAxis.xHomed) AND (_fbYAxis.xDone OR _fbYAxis.xHomed) THEN
|
|
_iSSM := 20;
|
|
END_IF
|
|
|
|
IF _fbXAxis.xError OR _fbYAxis.xError THEN
|
|
M_CmdAbort();
|
|
END_IF
|
|
|
|
// Start moving to position 0,0
|
|
20:
|
|
// Enable X-Axis
|
|
IF (NOT _xAxisXMoved) THEN
|
|
CASE _fbXAxis.M_MoveAbs(lrTargetPos := 0.0) OF
|
|
E_CmdResult.ACCEPTED:
|
|
_xAxisXMoved := TRUE;
|
|
|
|
E_CmdResult.BUSY:
|
|
;
|
|
|
|
E_CmdResult.REJECTED,
|
|
E_CmdResult.ERROR:
|
|
M_CmdAbort();
|
|
END_CASE
|
|
END_IF
|
|
|
|
// Enable Y-Axis
|
|
IF (NOT _xAxisYMoved) THEN
|
|
CASE _fbYAxis.M_MoveAbs(lrTargetPos := 0.0) OF
|
|
E_CmdResult.ACCEPTED:
|
|
_xAxisYMoved := TRUE;
|
|
|
|
E_CmdResult.BUSY:
|
|
;
|
|
|
|
E_CmdResult.REJECTED,
|
|
E_CmdResult.ERROR:
|
|
M_CmdAbort();
|
|
END_CASE
|
|
END_IF
|
|
|
|
// Check if both could be enabled
|
|
IF _xAxisXMoved AND _xAxisYMoved THEN
|
|
_iSSM := 25;
|
|
END_IF
|
|
|
|
25:
|
|
// Move to 0 position
|
|
IF _fbXAxis.xDone AND _fbYAxis.xDone THEN
|
|
M_StateComplete();
|
|
END_IF
|
|
|
|
IF _fbXAxis.xError OR _fbYAxis.xError THEN
|
|
M_CmdAbort();
|
|
END_IF
|
|
END_CASE]]></ST>
|
|
</Implementation>
|
|
</Method>
|
|
</POU>
|
|
</TcPlcObject> |