|
|
|
|
@@ -28,44 +28,44 @@ VAR
|
|
|
|
|
// 1 - Enable
|
|
|
|
|
// 2 - Enable move
|
|
|
|
|
// 3 - Conf_Mess (Reset)
|
|
|
|
|
_uCtrl AT %Q* : U_KukaRobot_Ctrl;
|
|
|
|
|
stCtrl AT %Q* : ST_KukaRobot_Ctrl;
|
|
|
|
|
|
|
|
|
|
// 0 -> Robot job to do
|
|
|
|
|
// 1 -> PLC job which is finished
|
|
|
|
|
_uJobs AT %Q* : U_KukaRobot_Jobs;
|
|
|
|
|
stJobs AT %Q* : ST_KukaRobot_Jobs;
|
|
|
|
|
|
|
|
|
|
// 0 - Greifer nummer
|
|
|
|
|
// 1 - Drehteller Nummer
|
|
|
|
|
// 2 - Position Heizplatte
|
|
|
|
|
// 3 - Position Kuehlplatte
|
|
|
|
|
_abToolsAndPositions AT %Q* : ARRAY[0..3] OF BYTE;
|
|
|
|
|
abyToolsAndPositions AT %Q* : ARRAY[0..3] OF BYTE;
|
|
|
|
|
|
|
|
|
|
// Positions in um
|
|
|
|
|
_diOffsetPosX AT %Q* : DINT;
|
|
|
|
|
_diOffsetPosY AT %Q* : DINT;
|
|
|
|
|
_diThickness AT %Q* : DINT;
|
|
|
|
|
diOffsetPosX AT %Q* : DINT;
|
|
|
|
|
diOffsetPosY AT %Q* : DINT;
|
|
|
|
|
diThickness AT %Q* : DINT;
|
|
|
|
|
|
|
|
|
|
// Inputs
|
|
|
|
|
_uState AT %I* : U_KukaRobot_State;
|
|
|
|
|
_dwErrorBits AT %I* : DWORD;
|
|
|
|
|
_awJobStatesFromRobot AT %I* : ARRAY[0..1] OF WORD;
|
|
|
|
|
_eJobForPLC AT %I* : E_KukaRobot_JobNumerPLC;
|
|
|
|
|
stState AT %I* : ST_KukaRobot_State;
|
|
|
|
|
dwErrorBits AT %I* : DWORD;
|
|
|
|
|
awJobStatesFromRobot AT %I* : ARRAY[0..1] OF WORD;
|
|
|
|
|
eJobForPLC AT %I* : E_KukaRobot_JobNumerPLC;
|
|
|
|
|
|
|
|
|
|
_abMirrorToolsAndPositions AT %I* : ARRAY[0..3] OF BYTE;
|
|
|
|
|
{attribute 'analysis' := '-33'}
|
|
|
|
|
abyMirrorToolsAndPositions AT %I* : ARRAY[0..3] OF BYTE;
|
|
|
|
|
|
|
|
|
|
// Position mirrors
|
|
|
|
|
_diMirrorOffsetPosX AT %I* : DINT;
|
|
|
|
|
_diMirrorOffsetPosY AT %I* : DINT;
|
|
|
|
|
_diMirrorThickness AT %I* : DINT;
|
|
|
|
|
diMirrorOffsetPosX AT %I* : DINT;
|
|
|
|
|
diMirrorOffsetPosY AT %I* : DINT;
|
|
|
|
|
diMirrorThickness AT %I* : DINT;
|
|
|
|
|
{attribute 'analysis' := '+33'}
|
|
|
|
|
|
|
|
|
|
// Motortemperaturen
|
|
|
|
|
_asiMotorTemps1To4 AT %I* : ARRAY[0..3] OF SINT;
|
|
|
|
|
_asiMotorTemps5To8 AT %I* : ARRAY[0..3] OF SINT;
|
|
|
|
|
asiMotorTemps1To4 AT %I* : ARRAY[0..3] OF SINT;
|
|
|
|
|
asiMotorTemps5To8 AT %I* : ARRAY[0..3] OF SINT;
|
|
|
|
|
|
|
|
|
|
_ePlcJob :E_KukaRobot_JobNumerPLC;
|
|
|
|
|
|
|
|
|
|
_iState : INT;
|
|
|
|
|
|
|
|
|
|
_xRobotReady : BOOL;
|
|
|
|
|
|
|
|
|
|
_xError : BOOL;
|
|
|
|
|
@@ -91,21 +91,21 @@ END_VAR
|
|
|
|
|
// =============
|
|
|
|
|
|
|
|
|
|
_fbAlarmNotInExtMode(
|
|
|
|
|
xActive:= (NOT _uState.stState.bExt),
|
|
|
|
|
xActive:= (NOT stState.bExt),
|
|
|
|
|
xRelease:= TRUE,
|
|
|
|
|
xAcknowledge:= xConfirmAlarms,
|
|
|
|
|
timOnDelay:= T#0S,
|
|
|
|
|
timOffDelay:= T#0S);
|
|
|
|
|
|
|
|
|
|
_fbAlarmEStopActive(
|
|
|
|
|
xActive:= (NOT _uState.stState.bAlarmStop),
|
|
|
|
|
xActive:= (NOT stState.bAlarmStop),
|
|
|
|
|
xRelease:= TRUE,
|
|
|
|
|
xAcknowledge:= xConfirmAlarms,
|
|
|
|
|
timOnDelay:= T#0S,
|
|
|
|
|
timOffDelay:= T#0S);
|
|
|
|
|
|
|
|
|
|
_fbAlarmOperatorSafetyNotOk(
|
|
|
|
|
xActive:= (NOT _uState.stState.bUserSAF) AND (_uState.stState.bExt),
|
|
|
|
|
xActive:= (NOT stState.bUserSAF) AND (stState.bExt),
|
|
|
|
|
xRelease:= TRUE,
|
|
|
|
|
xAcknowledge:= xConfirmAlarms,
|
|
|
|
|
timOnDelay:= T#0S,
|
|
|
|
|
@@ -126,12 +126,12 @@ _fbAlarmRobotErrorResetTimeout(
|
|
|
|
|
// Check if robot is ready
|
|
|
|
|
// =======================
|
|
|
|
|
|
|
|
|
|
_xRobotReady := _uState.stState.bExt
|
|
|
|
|
AND _uState.stState.bAlarmStop
|
|
|
|
|
AND _uState.stState.bUserSAF
|
|
|
|
|
AND _uState.stState.bPeriRdy
|
|
|
|
|
AND _uState.stState.bIOActConf
|
|
|
|
|
AND (NOT _uState.stState.bStopMess);
|
|
|
|
|
_xRobotReady := stState.bExt
|
|
|
|
|
AND stState.bAlarmStop
|
|
|
|
|
AND stState.bUserSAF
|
|
|
|
|
AND stState.bPeriRdy
|
|
|
|
|
AND stState.bIOActConf
|
|
|
|
|
AND (NOT stState.bStopMess);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// =============================
|
|
|
|
|
@@ -145,7 +145,7 @@ SUPER^();
|
|
|
|
|
// ==============================
|
|
|
|
|
|
|
|
|
|
// Allow robot to be moved with programming pendant
|
|
|
|
|
_uCtrl.stCtrl.bNotDisableAxes := 1;
|
|
|
|
|
stCtrl.bNotDisableAxes := 1;
|
|
|
|
|
|
|
|
|
|
// Disable move if in T2 for safety reasons
|
|
|
|
|
// IF _uState.stState.bT2 THEN
|
|
|
|
|
@@ -157,10 +157,10 @@ _uCtrl.stCtrl.bNotDisableAxes := 1;
|
|
|
|
|
// Write unit feedbacks outputs
|
|
|
|
|
// ============================
|
|
|
|
|
|
|
|
|
|
_uCtrl.stCtrl.bDoorEtcher1Open := stUnitFeedbacks.xDoorEtcher1Open;
|
|
|
|
|
_uCtrl.stCtrl.bDoorEtcher2Open := stUnitFeedbacks.xDoorEtcher2Open;
|
|
|
|
|
_uCtrl.stCtrl.bDoorHVTestHotOpen := stUnitFeedbacks.xDoorHVTestHotOpen;
|
|
|
|
|
_uCtrl.stCtrl.bDoorHVTestColdOpen := stUnitFeedbacks.xDoorHVTestColdOpen;
|
|
|
|
|
stCtrl.bDoorEtcher1Open := stUnitFeedbacks.xDoorEtcher1Open;
|
|
|
|
|
stCtrl.bDoorEtcher2Open := stUnitFeedbacks.xDoorEtcher2Open;
|
|
|
|
|
stCtrl.bDoorHVTestHotOpen := stUnitFeedbacks.xDoorHVTestHotOpen;
|
|
|
|
|
stCtrl.bDoorHVTestColdOpen := stUnitFeedbacks.xDoorHVTestColdOpen;
|
|
|
|
|
|
|
|
|
|
M_HandlePLCJobs();
|
|
|
|
|
|
|
|
|
|
@@ -196,16 +196,16 @@ _stSMConfig.xAbortingDisabled := TRUE;]]></ST>
|
|
|
|
|
]]></Declaration>
|
|
|
|
|
<Implementation>
|
|
|
|
|
<ST><![CDATA[// Reset all relevant robot control signals
|
|
|
|
|
_uCtrl.stCtrl.bConfMess := 0;
|
|
|
|
|
_uCtrl.stCtrl.bExtStart := 0;
|
|
|
|
|
_uCtrl.stCtrl.bEnableAxes := 0;
|
|
|
|
|
_uCtrl.stCtrl.bEnableMove := 0;
|
|
|
|
|
_uCtrl.stCtrl.bNotDisableAxes := 1;
|
|
|
|
|
_uCtrl.stCtrl.bAbortJob := 1;
|
|
|
|
|
stCtrl.bConfMess := 0;
|
|
|
|
|
stCtrl.bExtStart := 0;
|
|
|
|
|
stCtrl.bEnableAxes := 0;
|
|
|
|
|
stCtrl.bEnableMove := 0;
|
|
|
|
|
stCtrl.bNotDisableAxes := 1;
|
|
|
|
|
stCtrl.bAbortJob := 1;
|
|
|
|
|
|
|
|
|
|
// Reset robot program and plc job number
|
|
|
|
|
_uJobs.stJobs.wJobNrForRobot := E_KukaRobot_JobNumberRobot.NO_JOB;
|
|
|
|
|
_uJobs.stJobs.wFinishedJobNrFromPlc := E_KukaRobot_JobNumerPLC.NO_JOB;]]></ST>
|
|
|
|
|
stJobs.wJobNrForRobot := E_KukaRobot_JobNumberRobot.NO_JOB;
|
|
|
|
|
stJobs.wFinishedJobNrFromPlc := E_KukaRobot_JobNumerPLC.NO_JOB;]]></ST>
|
|
|
|
|
</Implementation>
|
|
|
|
|
</Method>
|
|
|
|
|
<Method Name="M_Clearing" Id="{e6152288-8c7d-4b38-bfbc-4f861d76ccc5}">
|
|
|
|
|
@@ -215,8 +215,8 @@ VAR_INST
|
|
|
|
|
END_VAR]]></Declaration>
|
|
|
|
|
<Implementation>
|
|
|
|
|
<ST><![CDATA[_tonWait(IN := TRUE, PT := T#1S);
|
|
|
|
|
_uCtrl.stCtrl.bEnableMove := 1;
|
|
|
|
|
_uCtrl.stCtrl.bAbortJob := 0;
|
|
|
|
|
stCtrl.bEnableMove := 1;
|
|
|
|
|
stCtrl.bAbortJob := 0;
|
|
|
|
|
|
|
|
|
|
IF _tonWait.Q THEN
|
|
|
|
|
_tonWait(IN := FALSE, PT := T#1S);
|
|
|
|
|
@@ -234,8 +234,8 @@ END_IF
|
|
|
|
|
E_KukaRobot_JobNumberRobot.WARMUP:
|
|
|
|
|
CASE _iSSM OF
|
|
|
|
|
0: // Wait for program to end
|
|
|
|
|
IF _awJobStatesFromRobot[0] = 0 THEN
|
|
|
|
|
IF _asiMotorTemps1To4[3] < 40 THEN
|
|
|
|
|
IF awJobStatesFromRobot[0] = 0 THEN
|
|
|
|
|
IF asiMotorTemps1To4[3] < 40 THEN
|
|
|
|
|
_iSSM := 10;
|
|
|
|
|
ELSE
|
|
|
|
|
_eCmd := E_PackMLCmd.COMPLETE;
|
|
|
|
|
@@ -243,17 +243,20 @@ END_IF
|
|
|
|
|
END_IF
|
|
|
|
|
|
|
|
|
|
10: // Write robot number 0
|
|
|
|
|
_uJobs.stJobs.wJobNrForRobot := stJobParams.eJob;
|
|
|
|
|
IF _awJobStatesFromRobot[0] = _uJobs.stJobs.wJobNrForRobot THEN
|
|
|
|
|
_uJobs.stJobs.wJobNrForRobot := E_KukaRobot_JobNumberRobot.NO_JOB;
|
|
|
|
|
stJobs.wJobNrForRobot := stJobParams.eJob;
|
|
|
|
|
IF awJobStatesFromRobot[0] = stJobs.wJobNrForRobot THEN
|
|
|
|
|
stJobs.wJobNrForRobot := E_KukaRobot_JobNumberRobot.NO_JOB;
|
|
|
|
|
_iSSM := 0;
|
|
|
|
|
END_IF
|
|
|
|
|
|
|
|
|
|
ELSE
|
|
|
|
|
;
|
|
|
|
|
END_CASE
|
|
|
|
|
|
|
|
|
|
ELSE
|
|
|
|
|
// Wait for robot to be done
|
|
|
|
|
IF _awJobStatesFromRobot[0] = 0 THEN
|
|
|
|
|
_uJobs.stJobs.wJobNrForRobot := E_KukaRobot_JobNumberRobot.NO_JOB;
|
|
|
|
|
IF awJobStatesFromRobot[0] = 0 THEN
|
|
|
|
|
stJobs.wJobNrForRobot := E_KukaRobot_JobNumberRobot.NO_JOB;
|
|
|
|
|
_eCmd := E_PackMLCmd.COMPLETE;
|
|
|
|
|
END_IF
|
|
|
|
|
END_CASE
|
|
|
|
|
@@ -264,35 +267,35 @@ END_CASE
|
|
|
|
|
<Method Name="M_HandlePLCJobs" Id="{4e3710b4-697b-42e8-86b1-31b19b3023db}">
|
|
|
|
|
<Declaration><![CDATA[METHOD M_HandlePLCJobs
|
|
|
|
|
VAR_INST
|
|
|
|
|
_rtrigNewJob : R_TRIG;
|
|
|
|
|
_rtNewJob : R_TRIG;
|
|
|
|
|
END_VAR
|
|
|
|
|
]]></Declaration>
|
|
|
|
|
<Implementation>
|
|
|
|
|
<ST><![CDATA[_rtrigNewJob(CLK := (_eJobForPLC <> E_KukaRobot_JobNumberRobot.NO_JOB));
|
|
|
|
|
<ST><![CDATA[_rtNewJob(CLK := (eJobForPLC <> E_KukaRobot_JobNumerPLC.NO_JOB));
|
|
|
|
|
|
|
|
|
|
IF _rtrigNewJob.Q THEN
|
|
|
|
|
eCmdFromRobot := _eJobForPLC;
|
|
|
|
|
IF _rtNewJob.Q THEN
|
|
|
|
|
eCmdFromRobot := eJobForPLC;
|
|
|
|
|
xNewCmdRequested := TRUE;
|
|
|
|
|
END_IF
|
|
|
|
|
|
|
|
|
|
IF xNewCmdRequested AND xAckPLCCmd THEN
|
|
|
|
|
IF xAckPLCCmd THEN
|
|
|
|
|
xNewCmdRequested := FALSE;
|
|
|
|
|
|
|
|
|
|
IF (NOT xPLCJobFailed) THEN
|
|
|
|
|
_uJobs.stJobs.wFinishedJobNrFromPlc := _eJobForPLC;
|
|
|
|
|
stJobs.wFinishedJobNrFromPlc := eJobForPLC;
|
|
|
|
|
ELSE
|
|
|
|
|
_uJobs.stJobs.wFinishedJobNrFromPlc := E_KukaRobot_JobNumerPLC.JOB_FAILED;
|
|
|
|
|
stJobs.wFinishedJobNrFromPlc := E_KukaRobot_JobNumerPLC.JOB_FAILED;
|
|
|
|
|
END_IF
|
|
|
|
|
|
|
|
|
|
eCmdFromRobot := E_KukaRobot_JobNumberRobot.NO_JOB;
|
|
|
|
|
eCmdFromRobot := E_KukaRobot_JobNumerPLC.NO_JOB;
|
|
|
|
|
END_IF
|
|
|
|
|
|
|
|
|
|
// Reset plc job done
|
|
|
|
|
IF (_eJobForPLC = E_KukaRobot_JobNumberRobot.NO_JOB)
|
|
|
|
|
AND (_uJobs.stJobs.wFinishedJobNrFromPlc <> E_KukaRobot_JobNumberRobot.NO_JOB)
|
|
|
|
|
IF (eJobForPLC = E_KukaRobot_JobNumerPLC.NO_JOB)
|
|
|
|
|
AND (stJobs.wFinishedJobNrFromPlc <> E_KukaRobot_JobNumberRobot.NO_JOB)
|
|
|
|
|
AND (NOT xPLCJobFailed)
|
|
|
|
|
THEN
|
|
|
|
|
_uJobs.stJobs.wFinishedJobNrFromPlc := E_KukaRobot_JobNumberRobot.NO_JOB;
|
|
|
|
|
stJobs.wFinishedJobNrFromPlc := E_KukaRobot_JobNumerPLC.NO_JOB;
|
|
|
|
|
END_IF]]></ST>
|
|
|
|
|
</Implementation>
|
|
|
|
|
</Method>
|
|
|
|
|
@@ -307,9 +310,9 @@ END_IF]]></ST>
|
|
|
|
|
<Declaration><![CDATA[METHOD PROTECTED M_Holding
|
|
|
|
|
]]></Declaration>
|
|
|
|
|
<Implementation>
|
|
|
|
|
<ST><![CDATA[_uCtrl.stCtrl.bEnableMove := 0;
|
|
|
|
|
<ST><![CDATA[stCtrl.bEnableMove := 0;
|
|
|
|
|
|
|
|
|
|
IF _uState.stState.bRobStopped THEN
|
|
|
|
|
IF stState.bRobStopped THEN
|
|
|
|
|
M_StateComplete();
|
|
|
|
|
END_IF
|
|
|
|
|
]]></ST>
|
|
|
|
|
@@ -333,37 +336,37 @@ END_VAR]]></Declaration>
|
|
|
|
|
CASE _iSSM OF
|
|
|
|
|
// Check if com interface is ok
|
|
|
|
|
0:
|
|
|
|
|
_uCtrl.stCtrl.bEnableMove := 1;
|
|
|
|
|
_uCtrl.stCtrl.bConfMess := 1;
|
|
|
|
|
stCtrl.bEnableMove := 1;
|
|
|
|
|
stCtrl.bConfMess := 1;
|
|
|
|
|
_tonTimeout.IN := TRUE;
|
|
|
|
|
|
|
|
|
|
IF _uState.stState.bIOActConf THEN
|
|
|
|
|
_uCtrl.stCtrl.bConfMess := 0;
|
|
|
|
|
IF stState.bIOActConf THEN
|
|
|
|
|
stCtrl.bConfMess := 0;
|
|
|
|
|
_iSSM := 10;
|
|
|
|
|
END_IF
|
|
|
|
|
|
|
|
|
|
// Timeout io_Act_conf ready
|
|
|
|
|
IF _tonTimeout.Q THEN
|
|
|
|
|
_uCtrl.stCtrl.bConfMess := 0;
|
|
|
|
|
stCtrl.bConfMess := 0;
|
|
|
|
|
_tonTimeout(IN := FALSE);
|
|
|
|
|
_eCmd := E_PackMLCmd.ABORT;
|
|
|
|
|
END_IF
|
|
|
|
|
|
|
|
|
|
// Enable axes
|
|
|
|
|
10:
|
|
|
|
|
_uCtrl.stCtrl.bEnableAxes := 1;
|
|
|
|
|
stCtrl.bEnableAxes := 1;
|
|
|
|
|
_tonTimeout.IN := TRUE;
|
|
|
|
|
|
|
|
|
|
// Wait for drives to be ready
|
|
|
|
|
IF _uState.stState.bPeriRdy THEN
|
|
|
|
|
_uCtrl.stCtrl.bEnableAxes := 0;
|
|
|
|
|
IF stState.bPeriRdy THEN
|
|
|
|
|
stCtrl.bEnableAxes := 0;
|
|
|
|
|
_tonTimeout(IN := FALSE);
|
|
|
|
|
_iSSM := 20;
|
|
|
|
|
END_IF
|
|
|
|
|
|
|
|
|
|
// Timeout drives ready
|
|
|
|
|
IF _tonTimeout.Q THEN
|
|
|
|
|
_uCtrl.stCtrl.bEnableAxes := 0;
|
|
|
|
|
stCtrl.bEnableAxes := 0;
|
|
|
|
|
_fbAlarmDrivesEnableTimeout.xRelease := TRUE;
|
|
|
|
|
_tonTimeout(IN := FALSE);
|
|
|
|
|
_eCmd := E_PackMLCmd.ABORT;
|
|
|
|
|
@@ -371,12 +374,12 @@ CASE _iSSM OF
|
|
|
|
|
|
|
|
|
|
// Reset move stop
|
|
|
|
|
20:
|
|
|
|
|
_uCtrl.stCtrl.bConfMess := 1;
|
|
|
|
|
stCtrl.bConfMess := 1;
|
|
|
|
|
_tonTimeout.IN := TRUE;
|
|
|
|
|
|
|
|
|
|
// Wait for errors to be reset
|
|
|
|
|
IF (NOT _uState.stState.bStopMess) THEN
|
|
|
|
|
_uCtrl.stCtrl.bConfMess := 0;
|
|
|
|
|
IF (NOT stState.bStopMess) THEN
|
|
|
|
|
stCtrl.bConfMess := 0;
|
|
|
|
|
_tonTimeout(IN := FALSE);
|
|
|
|
|
_iSSM := 30;
|
|
|
|
|
END_IF
|
|
|
|
|
@@ -390,7 +393,7 @@ CASE _iSSM OF
|
|
|
|
|
|
|
|
|
|
// Start program
|
|
|
|
|
30:
|
|
|
|
|
_uCtrl.stCtrl.bExtStart := 1;
|
|
|
|
|
stCtrl.bExtStart := 1;
|
|
|
|
|
_tonTimeout.IN := TRUE;
|
|
|
|
|
|
|
|
|
|
IF _tonTimeout.Q THEN
|
|
|
|
|
@@ -398,12 +401,14 @@ CASE _iSSM OF
|
|
|
|
|
_eCmd := E_PackMLCmd.ABORT;
|
|
|
|
|
END_IF
|
|
|
|
|
|
|
|
|
|
IF _uState.stState.bProAct THEN
|
|
|
|
|
_uCtrl.stCtrl.bExtStart := 0;
|
|
|
|
|
IF stState.bProAct THEN
|
|
|
|
|
stCtrl.bExtStart := 0;
|
|
|
|
|
_tonTimeout(IN := FALSE);
|
|
|
|
|
M_StateComplete();
|
|
|
|
|
END_IF
|
|
|
|
|
|
|
|
|
|
ELSE
|
|
|
|
|
;
|
|
|
|
|
END_CASE]]></ST>
|
|
|
|
|
</Implementation>
|
|
|
|
|
</Method>
|
|
|
|
|
@@ -416,7 +421,7 @@ END_VAR]]></Declaration>
|
|
|
|
|
<ST><![CDATA[CASE _iSSM OF
|
|
|
|
|
// Check job parameters
|
|
|
|
|
0:
|
|
|
|
|
IF (_stJobParams.byGripperNumber < 0) OR (_stJobParams.byGripperNumber > 4) // Wrong gripper number
|
|
|
|
|
IF (_stJobParams.byGripperNumber > 4) // Wrong gripper number
|
|
|
|
|
//OR (_stJobParams.byChuckNumber < 1) OR (_stJobParams.byChuckNumber > 6) // Wrong chuck number
|
|
|
|
|
OR (_stJobParams.byGripperSide < 1) OR (_stJobParams.byGripperSide > 6) // Wrong gripper side
|
|
|
|
|
THEN
|
|
|
|
|
@@ -433,38 +438,38 @@ END_VAR]]></Declaration>
|
|
|
|
|
// Transfer job data to robot
|
|
|
|
|
10:
|
|
|
|
|
// Pos x in um
|
|
|
|
|
_diOffsetPosX := REAL_TO_DINT(_stJobParams.rPosX * 1000);
|
|
|
|
|
diOffsetPosX := REAL_TO_DINT(_stJobParams.rPosX * 1000);
|
|
|
|
|
|
|
|
|
|
// Pos y in um
|
|
|
|
|
_diOffsetPosY := REAL_TO_DINT(_stJobParams.rPosY * 1000);
|
|
|
|
|
diOffsetPosY := REAL_TO_DINT(_stJobParams.rPosY * 1000);
|
|
|
|
|
|
|
|
|
|
// Thickness
|
|
|
|
|
_diThickness := REAL_TO_DINT(_stJobParams.rThickness * 1000);
|
|
|
|
|
diThickness := REAL_TO_DINT(_stJobParams.rThickness * 1000);
|
|
|
|
|
|
|
|
|
|
// Gripper side
|
|
|
|
|
_uCtrl.stCtrl.byGripperSide := _stJobParams.byGripperSide;
|
|
|
|
|
stCtrl.byGripperSide := _stJobParams.byGripperSide;
|
|
|
|
|
|
|
|
|
|
// Scan QR code
|
|
|
|
|
IF stJobParams.xScanQRCode THEN
|
|
|
|
|
_uCtrl.stCtrl.bScanQRCode := 1;
|
|
|
|
|
stCtrl.bScanQRCode := 1;
|
|
|
|
|
ELSE
|
|
|
|
|
_uCtrl.stCtrl.bScanQRCode := 0;
|
|
|
|
|
stCtrl.bScanQRCode := 0;
|
|
|
|
|
END_IF
|
|
|
|
|
|
|
|
|
|
// Position on hotplate
|
|
|
|
|
_abToolsAndPositions[2] := stJobParams.byPlaceOnHotplate;
|
|
|
|
|
abyToolsAndPositions[2] := stJobParams.byPlaceOnHotplate;
|
|
|
|
|
|
|
|
|
|
// Position on coolplate
|
|
|
|
|
_abToolsAndPositions[3] := stJobParams.byPlaceOnCoolPlate;
|
|
|
|
|
abyToolsAndPositions[3] := stJobParams.byPlaceOnCoolPlate;
|
|
|
|
|
|
|
|
|
|
// Gripper number
|
|
|
|
|
_abToolsAndPositions[0] := stJobParams.byGripperNumber;
|
|
|
|
|
abyToolsAndPositions[0] := stJobParams.byGripperNumber;
|
|
|
|
|
|
|
|
|
|
// Chuck for etcher to load
|
|
|
|
|
_abToolsAndPositions[1] := stJobParams.byChuckNumber;
|
|
|
|
|
abyToolsAndPositions[1] := stJobParams.byChuckNumber;
|
|
|
|
|
|
|
|
|
|
// Write robot job number
|
|
|
|
|
_uJobs.stJobs.wJobNrForRobot := DINT_TO_WORD(_stJobParams.eJob);
|
|
|
|
|
stJobs.wJobNrForRobot := DINT_TO_WORD(_stJobParams.eJob);
|
|
|
|
|
|
|
|
|
|
// Safety reset timout fb
|
|
|
|
|
_tonTimeout(IN := FALSE);
|
|
|
|
|
@@ -476,8 +481,8 @@ END_VAR]]></Declaration>
|
|
|
|
|
_tonTimeout(IN := TRUE, PT := T#5S);
|
|
|
|
|
|
|
|
|
|
// If job number is read back correct, go to next state
|
|
|
|
|
IF _awJobStatesFromRobot[0] = _uJobs.stJobs.wJobNrForRobot THEN
|
|
|
|
|
_uJobs.stJobs.wJobNrForRobot := E_KukaRobot_JobNumberRobot.NO_JOB;
|
|
|
|
|
IF awJobStatesFromRobot[0] = stJobs.wJobNrForRobot THEN
|
|
|
|
|
stJobs.wJobNrForRobot := E_KukaRobot_JobNumberRobot.NO_JOB;
|
|
|
|
|
_tonTimeout(IN := FALSE);
|
|
|
|
|
M_StateComplete();
|
|
|
|
|
END_IF
|
|
|
|
|
@@ -489,10 +494,13 @@ END_VAR]]></Declaration>
|
|
|
|
|
END_IF
|
|
|
|
|
|
|
|
|
|
// Robot has error
|
|
|
|
|
IF _dwErrorBits <> 0 THEN
|
|
|
|
|
IF dwErrorBits <> 0 THEN
|
|
|
|
|
_tonTimeout(IN := FALSE);
|
|
|
|
|
_eCmd := E_PackMLCmd.ABORT;
|
|
|
|
|
END_IF
|
|
|
|
|
|
|
|
|
|
ELSE
|
|
|
|
|
;
|
|
|
|
|
END_CASE
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
@@ -509,11 +517,11 @@ END_CASE
|
|
|
|
|
]]></Declaration>
|
|
|
|
|
<Implementation>
|
|
|
|
|
<ST><![CDATA[// _uCtrl.stCtrl.bConfMess := 0;
|
|
|
|
|
_uCtrl.stCtrl.bEnableAxes := 0;
|
|
|
|
|
_uCtrl.stCtrl.bNotDisableAxes := 1;
|
|
|
|
|
_uCtrl.stCtrl.bExtStart := 0;
|
|
|
|
|
stCtrl.bEnableAxes := 0;
|
|
|
|
|
stCtrl.bNotDisableAxes := 1;
|
|
|
|
|
stCtrl.bExtStart := 0;
|
|
|
|
|
|
|
|
|
|
_uJobs.stJobs.wJobNrForRobot := E_KukaRobot_JobNumberRobot.NO_JOB;
|
|
|
|
|
stJobs.wJobNrForRobot := E_KukaRobot_JobNumberRobot.NO_JOB;
|
|
|
|
|
_ePlcJob := E_KukaRobot_JobNumerPLC.NO_JOB;]]></ST>
|
|
|
|
|
</Implementation>
|
|
|
|
|
</Method>
|
|
|
|
|
@@ -541,33 +549,33 @@ END_VAR]]></Declaration>
|
|
|
|
|
|
|
|
|
|
CASE _iSSM OF
|
|
|
|
|
0:
|
|
|
|
|
_uCtrl.stCtrl.bEnableMove := 1;
|
|
|
|
|
_uCtrl.stCtrl.bEnableAxes := 1;
|
|
|
|
|
stCtrl.bEnableMove := 1;
|
|
|
|
|
stCtrl.bEnableAxes := 1;
|
|
|
|
|
_tonTimeout.IN := TRUE;
|
|
|
|
|
|
|
|
|
|
// Wait for drives to be ready
|
|
|
|
|
IF _uState.stState.bPeriRdy THEN
|
|
|
|
|
_uCtrl.stCtrl.bEnableAxes := 0;
|
|
|
|
|
IF stState.bPeriRdy THEN
|
|
|
|
|
stCtrl.bEnableAxes := 0;
|
|
|
|
|
_tonTimeout(IN := FALSE);
|
|
|
|
|
_iSSM := 10;
|
|
|
|
|
END_IF
|
|
|
|
|
|
|
|
|
|
// Timeout drives ready
|
|
|
|
|
IF _tonTimeout.Q THEN
|
|
|
|
|
_uCtrl.stCtrl.bEnableAxes := 0;
|
|
|
|
|
stCtrl.bEnableAxes := 0;
|
|
|
|
|
_fbAlarmDrivesEnableTimeout.xRelease := TRUE;
|
|
|
|
|
_tonTimeout(IN := FALSE);
|
|
|
|
|
_eCmd := E_PackMLCmd.ABORT;
|
|
|
|
|
END_IF
|
|
|
|
|
|
|
|
|
|
10:
|
|
|
|
|
_uCtrl.stCtrl.bConfMess := 1;
|
|
|
|
|
stCtrl.bConfMess := 1;
|
|
|
|
|
|
|
|
|
|
_tonTimeout.IN := TRUE;
|
|
|
|
|
|
|
|
|
|
// Wait for errors to be reset
|
|
|
|
|
IF (NOT _uState.stState.bStopMess) THEN
|
|
|
|
|
_uCtrl.stCtrl.bConfMess := 0;
|
|
|
|
|
IF (NOT stState.bStopMess) THEN
|
|
|
|
|
stCtrl.bConfMess := 0;
|
|
|
|
|
_tonTimeout(IN := FALSE);
|
|
|
|
|
_iSSM := 20;
|
|
|
|
|
END_IF
|
|
|
|
|
@@ -581,7 +589,7 @@ CASE _iSSM OF
|
|
|
|
|
|
|
|
|
|
20:
|
|
|
|
|
// Start main program
|
|
|
|
|
_uCtrl.stCtrl.bExtStart := 1;
|
|
|
|
|
stCtrl.bExtStart := 1;
|
|
|
|
|
|
|
|
|
|
_tonTimeout.IN := TRUE;
|
|
|
|
|
|
|
|
|
|
@@ -590,12 +598,14 @@ CASE _iSSM OF
|
|
|
|
|
_eCmd := E_PackMLCmd.ABORT;
|
|
|
|
|
END_IF
|
|
|
|
|
|
|
|
|
|
IF _uState.stState.bProAct THEN
|
|
|
|
|
_uCtrl.stCtrl.bExtStart := 0;
|
|
|
|
|
IF stState.bProAct THEN
|
|
|
|
|
stCtrl.bExtStart := 0;
|
|
|
|
|
_tonTimeout(IN := FALSE);
|
|
|
|
|
M_StateComplete();
|
|
|
|
|
END_IF
|
|
|
|
|
|
|
|
|
|
ELSE
|
|
|
|
|
;
|
|
|
|
|
END_CASE]]></ST>
|
|
|
|
|
</Implementation>
|
|
|
|
|
</Method>
|
|
|
|
|
|