457 lines
13 KiB
XML
457 lines
13 KiB
XML
<?xml version="1.0" encoding="utf-8"?>
|
|
<TcPlcObject Version="1.1.0.1">
|
|
<POU Name="FB_KukaRobot" Id="{6ca3e82a-4047-4132-872d-1f49057f08d6}" SpecialFunc="None">
|
|
<Declaration><![CDATA[// Positionen in um <=> 0.001mm
|
|
FUNCTION_BLOCK FINAL FB_KukaRobot EXTENDS FB_PackMLGeneric
|
|
VAR_INPUT
|
|
stJobParams : ST_KukaRobot_JobParams;
|
|
|
|
// Feedbacks from other units
|
|
// like if a door is open and such things
|
|
stUnitFeedbacks : ST_KukaRobot_UnitFeedbacks;
|
|
|
|
xReleaseAlarms : BOOL;
|
|
xConfirmAlarms : BOOL;
|
|
END_VAR
|
|
VAR_OUTPUT
|
|
xReady : BOOL;
|
|
xError : BOOL;
|
|
END_VAR
|
|
VAR
|
|
// 0 - Nicht aus
|
|
// 1 - Enable
|
|
// 2 - Enable move
|
|
// 3 - Conf_Mess (Reset)
|
|
_uCtrl AT %Q* : U_KukaRobot_Ctrl;
|
|
|
|
// 0 -> Robot job to do
|
|
// 1 -> PLC job which is finished
|
|
_uJobs AT %Q* : U_KukaRobot_Jobs;
|
|
|
|
// 0 - Greifer nummer
|
|
// 1 - Drehteller Nummer
|
|
// 2 - Position Heizplatte
|
|
// 3 - Position Kuehlplatte
|
|
_abToolsAndPositions AT %Q* : ARRAY[0..3] OF BYTE;
|
|
|
|
// Positions in um
|
|
_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;
|
|
_dwJobForPLC AT %I* : DWORD;
|
|
|
|
_abMirrorToolsAndPositions AT %I* : ARRAY[0..3] OF BYTE;
|
|
|
|
// Position mirrors
|
|
_diMirrorOffsetPosX AT %I* : DINT;
|
|
_diMirrorOffsetPosY AT %I* : DINT;
|
|
_diMirrorThickness AT %I* : DINT;
|
|
|
|
// Motortemperaturen
|
|
_asiMotorTemps1To4 AT %I* : ARRAY[0..3] OF SINT;
|
|
_asiMotorTemps5To8 AT %I* : ARRAY[0..3] OF SINT;
|
|
|
|
_ePlcJob :E_KukaRobot_JobNumerPLC;
|
|
|
|
_xRobotReady : BOOL;
|
|
|
|
_xError : BOOL;
|
|
|
|
// Internal job params will be copied from input
|
|
// only from IDLE -> STARTING
|
|
_stJobParams : ST_KukaRobot_JobParams;
|
|
|
|
// ======
|
|
// Alarms
|
|
// ======
|
|
|
|
_fbAlarmNotInExtMode : FB_AlarmMessage(stEventEntry := TC_EVENTS.KukaRobot.NotInExtMode, xWithConfirmation := FALSE);
|
|
_fbAlarmEStopActive : FB_AlarmMessage(stEventEntry := TC_EVENTS.KukaRobot.EStopActive, xWithConfirmation := FALSE);
|
|
_fbAlarmOperatorSafetyNotOk : FB_AlarmMessage(stEventEntry := TC_EVENTS.KukaRobot.OperatorSafetyNotOk, xWithConfirmation := FALSE);
|
|
_fbAlarmRobotErrorResetTimeout : FB_AlarmMessage(stEventEntry := TC_EVENTS.KukaRobot.ErrorResetTimeout, xWithConfirmation := TRUE);
|
|
_fbAlarmDrivesEnableTimeout : FB_AlarmMessage(stEventEntry := TC_EVENTS.KukaRobot.DrivesEnableTimeout, xWithConfirmation := TRUE);
|
|
END_VAR
|
|
]]></Declaration>
|
|
<Implementation>
|
|
<ST><![CDATA[// =============
|
|
// Handle alarms
|
|
// =============
|
|
|
|
_fbAlarmNotInExtMode(
|
|
xActive:= (NOT _uState.stState.bExt),
|
|
xRelease:= TRUE,
|
|
xAcknowledge:= xConfirmAlarms,
|
|
timOnDelay:= T#0S,
|
|
timOffDelay:= T#0S);
|
|
|
|
_fbAlarmEStopActive(
|
|
xActive:= (NOT _uState.stState.bAlarmStop),
|
|
xRelease:= TRUE,
|
|
xAcknowledge:= xConfirmAlarms,
|
|
timOnDelay:= T#0S,
|
|
timOffDelay:= T#0S);
|
|
|
|
_fbAlarmOperatorSafetyNotOk(
|
|
xActive:= (NOT _uState.stState.bUserSAF),
|
|
xRelease:= TRUE,
|
|
xAcknowledge:= xConfirmAlarms,
|
|
timOnDelay:= T#0S,
|
|
timOffDelay:= T#0S);
|
|
|
|
// Handled by M_Resetting
|
|
_fbAlarmDrivesEnableTimeout(
|
|
xRelease:= xReleaseAlarms,
|
|
xAcknowledge:= xConfirmAlarms);
|
|
|
|
// Handled by M_Resetting
|
|
_fbAlarmRobotErrorResetTimeout(
|
|
xRelease:= xReleaseAlarms,
|
|
xAcknowledge:= xConfirmAlarms);
|
|
|
|
|
|
// =======================
|
|
// 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);
|
|
|
|
|
|
// =============================
|
|
// Call isa88 base state machine
|
|
// =============================
|
|
SUPER^();
|
|
|
|
|
|
// ==============================
|
|
// Handle general control outputs
|
|
// ==============================
|
|
|
|
// Allow robot to be moved with programming pendant
|
|
_uCtrl.stCtrl.bNotDisableAxes := 1;
|
|
|
|
// Disable move if in T2 for safety reasons
|
|
IF _uState.stState.bT2 THEN
|
|
_uCtrl.stCtrl.bEnableMove := 0;
|
|
END_IF
|
|
|
|
|
|
// ============================
|
|
// Write unit feedbacks outputs
|
|
// ============================
|
|
|
|
|
|
|
|
|
|
// =================
|
|
// Write fb outputs
|
|
// =================
|
|
|
|
xReady := _xRobotReady;
|
|
xError := _xError;]]></ST>
|
|
</Implementation>
|
|
<Method Name="FB_Init" Id="{71a3f6f0-d3bb-4348-acc7-be533d7488d2}">
|
|
<Declaration><![CDATA[//FB_Init ist immer implizit verfügbar und wird primär für die Initialisierung verwendet.
|
|
//Der Rückgabewert wird nicht ausgewertet. Für gezielte Einflussnahme können Sie
|
|
//die Methoden explizit deklarieren und darin mit dem Standard-Initialisierungscode
|
|
//zusätzlichen Code bereitstellen. Sie können den Rückgabewert auswerten.
|
|
METHOD FB_Init: BOOL
|
|
VAR_INPUT
|
|
bInitRetains: BOOL; // TRUE: Die Retain-Variablen werden initialisiert (Reset warm / Reset kalt)
|
|
bInCopyCode: BOOL; // TRUE: Die Instanz wird danach in den Kopiercode kopiert (Online-Change)
|
|
END_VAR]]></Declaration>
|
|
<Implementation>
|
|
<ST><![CDATA[_stSMConfig.xStoppingDisabled := TRUE;
|
|
|
|
_stSMConfig.xCompletingDisabled := TRUE;
|
|
_stSMConfig.xCompletedDisabled := TRUE;
|
|
|
|
_stSMConfig.xAbortingDisabled := TRUE;]]></ST>
|
|
</Implementation>
|
|
</Method>
|
|
<Method Name="M_Aborted" Id="{0c0c17ce-5c1c-4640-9d39-f25b22309d5d}">
|
|
<Declaration><![CDATA[METHOD M_Aborted
|
|
]]></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;
|
|
|
|
// Reset robot program and plc job number
|
|
_uJobs.stJobs.wJobNrForRobot := E_KukaRobot_JobNumberRobot.NO_JOB;
|
|
_uJobs.stJobs.wFinishedJobNrFromPlc := E_KukaRobot_JobNumerPLC.NO_JOB;]]></ST>
|
|
</Implementation>
|
|
</Method>
|
|
<Method Name="M_Clearing" Id="{e6152288-8c7d-4b38-bfbc-4f861d76ccc5}">
|
|
<Declaration><![CDATA[METHOD M_Clearing
|
|
VAR_INST
|
|
_tonWait : TON;
|
|
END_VAR]]></Declaration>
|
|
<Implementation>
|
|
<ST><![CDATA[_tonWait(IN := TRUE, PT := T#1S);
|
|
_uCtrl.stCtrl.bEnableMove := 1;
|
|
_uCtrl.stCtrl.bAbortJob := 0;
|
|
|
|
IF _tonWait.Q THEN
|
|
_tonWait(IN := FALSE, PT := T#1S);
|
|
M_StateComplete();
|
|
END_IF
|
|
|
|
]]></ST>
|
|
</Implementation>
|
|
</Method>
|
|
<Method Name="M_Execute" Id="{bc86876c-a8db-4285-b95c-7c6858a2ca66}">
|
|
<Declaration><![CDATA[METHOD M_Execute
|
|
]]></Declaration>
|
|
<Implementation>
|
|
<ST><![CDATA[// Wait for robot to be done
|
|
IF _awJobStatesFromRobot[0] = 0 THEN
|
|
_eCmd := E_PackMLCmd.COMPLETE;
|
|
END_IF]]></ST>
|
|
</Implementation>
|
|
</Method>
|
|
<Method Name="M_Held" Id="{22ddbad0-0b66-427e-a2aa-f177f8adbc94}">
|
|
<Declaration><![CDATA[METHOD M_Held
|
|
]]></Declaration>
|
|
<Implementation>
|
|
<ST><![CDATA[]]></ST>
|
|
</Implementation>
|
|
</Method>
|
|
<Method Name="M_Holding" Id="{ff8c2c8e-1fe0-4c63-ad7b-790d72bd9217}">
|
|
<Declaration><![CDATA[METHOD M_Holding
|
|
]]></Declaration>
|
|
<Implementation>
|
|
<ST><![CDATA[]]></ST>
|
|
</Implementation>
|
|
</Method>
|
|
<Method Name="M_Idle" Id="{025437ab-beb0-4ad6-bdc3-468dec599ff4}">
|
|
<Declaration><![CDATA[METHOD M_Idle
|
|
]]></Declaration>
|
|
<Implementation>
|
|
<ST><![CDATA[_stJobParams := stJobParams;]]></ST>
|
|
</Implementation>
|
|
</Method>
|
|
<Method Name="M_Resetting" Id="{dfe4d36a-80aa-4364-bfd8-6ddd41636d59}">
|
|
<Declaration><![CDATA[METHOD M_Resetting
|
|
VAR_INST
|
|
_tonTimeout : TON;
|
|
END_VAR]]></Declaration>
|
|
<Implementation>
|
|
<ST><![CDATA[_tonTimeout(PT := T#10S);
|
|
|
|
CASE _iSSM OF
|
|
0:
|
|
_uCtrl.stCtrl.bEnableAxes := 1;
|
|
_tonTimeout.IN := TRUE;
|
|
|
|
// Wait for drives to be ready
|
|
IF _uState.stState.bPeriRdy THEN
|
|
_uCtrl.stCtrl.bEnableAxes := 0;
|
|
_tonTimeout(IN := FALSE);
|
|
_iSSM := 10;
|
|
END_IF
|
|
|
|
// Timeout drives ready
|
|
IF _tonTimeout.Q THEN
|
|
_uCtrl.stCtrl.bEnableAxes := 0;
|
|
_fbAlarmDrivesEnableTimeout.xRelease := TRUE;
|
|
_tonTimeout(IN := FALSE);
|
|
_eCmd := E_PackMLCmd.ABORT;
|
|
END_IF
|
|
|
|
10:
|
|
_uCtrl.stCtrl.bConfMess := 1;
|
|
|
|
_tonTimeout.IN := TRUE;
|
|
|
|
// Wait for errors to be reset
|
|
IF (NOT _uState.stState.bStopMess) THEN
|
|
_uCtrl.stCtrl.bConfMess := 0;
|
|
_tonTimeout(IN := FALSE);
|
|
_iSSM := 20;
|
|
END_IF
|
|
|
|
// Timeout reset errors
|
|
IF _tonTimeout.Q THEN
|
|
_fbAlarmRobotErrorResetTimeout.xRelease := TRUE;
|
|
_tonTimeout(IN := FALSE);
|
|
_eCmd := E_PackMLCmd.ABORT;
|
|
END_IF
|
|
|
|
20:
|
|
// Start main program
|
|
_uCtrl.stCtrl.bExtStart := 1;
|
|
|
|
_tonTimeout.IN := TRUE;
|
|
|
|
IF _tonTimeout.Q THEN
|
|
_tonTimeout(IN := FALSE);
|
|
_eCmd := E_PackMLCmd.ABORT;
|
|
END_IF
|
|
|
|
IF _uState.stState.bProAct THEN
|
|
_uCtrl.stCtrl.bExtStart := 0;
|
|
_tonTimeout(IN := FALSE);
|
|
M_StateComplete();
|
|
END_IF
|
|
|
|
END_CASE]]></ST>
|
|
</Implementation>
|
|
</Method>
|
|
<Method Name="M_Starting" Id="{011f85d8-1225-4288-876f-918df6e4f235}">
|
|
<Declaration><![CDATA[METHOD M_Starting
|
|
VAR_INST
|
|
_tonTimeout : TON;
|
|
END_VAR]]></Declaration>
|
|
<Implementation>
|
|
<ST><![CDATA[CASE _iSSM OF
|
|
// Check job parameters
|
|
0:
|
|
IF (_stJobParams.byGripperNumber < 0) OR (_stJobParams.byGripperNumber > 3) // Wrong gripper number
|
|
OR (_stJobParams.byChuckNumber < 1) OR (_stJobParams.byChuckNumber > 6) // Wrong chuck number
|
|
OR (_stJobParams.byGripperSide < 1) OR (_stJobParams.byGripperSide > 2) // Wrong gripper side
|
|
THEN
|
|
_eCmd := E_PackMLCmd.ABORT;
|
|
ELSE
|
|
_iSSM := 10;
|
|
END_IF
|
|
|
|
// Transfer job data to robot
|
|
10:
|
|
// Pos x in um
|
|
_diOffsetPosX := REAL_TO_DINT(_stJobParams.rPosX * 1000);
|
|
|
|
// Pos y in um
|
|
_diOffsetPosY := REAL_TO_DINT(_stJobParams.rPosY * 1000);
|
|
|
|
// Thickness
|
|
_diThickness := REAL_TO_DINT(_stJobParams.rThickness * 1000);
|
|
|
|
// Gripper side
|
|
IF _stJobParams.byGripperSide = 1 THEN
|
|
_uCtrl.stCtrl.bGripperSide1 := 1;
|
|
_uCtrl.stCtrl.bGripperSide2 := 0;
|
|
ELSIF _stJobParams.byGripperSide = 2 THEN
|
|
_uCtrl.stCtrl.bGripperSide1 := 0;
|
|
_uCtrl.stCtrl.bGripperSide2 := 1;
|
|
ELSE
|
|
_uCtrl.stCtrl.bGripperSide1 := 0;
|
|
_uCtrl.stCtrl.bGripperSide2 := 0;
|
|
END_IF
|
|
|
|
// Scan QR code
|
|
IF stJobParams.xScanQRCode THEN
|
|
_uCtrl.stCtrl.bScanQRCode := 1;
|
|
ELSE
|
|
_uCtrl.stCtrl.bScanQRCode := 0;
|
|
END_IF
|
|
|
|
// Position on hotplate
|
|
_abToolsAndPositions[2] := stJobParams.byPlaceOnHotplate;
|
|
|
|
// Position on coolplate
|
|
_abToolsAndPositions[3] := stJobParams.byPlaceOnCoolPlate;
|
|
|
|
// Gripper number
|
|
_abToolsAndPositions[0] := stJobParams.byGripperNumber;
|
|
|
|
// Chuck for etcher to load
|
|
_abToolsAndPositions[1] := stJobParams.byChuckNumber;
|
|
|
|
// Write robot job number
|
|
_uJobs.stJobs.wJobNrForRobot := DINT_TO_WORD(_stJobParams.eJob);
|
|
|
|
// Safety reset timout fb
|
|
_tonTimeout(IN := FALSE);
|
|
|
|
_iSSM := 20;
|
|
|
|
// Wait for robot program start
|
|
20:
|
|
_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;
|
|
_tonTimeout(IN := FALSE);
|
|
M_StateComplete();
|
|
END_IF
|
|
|
|
// Timeout occured
|
|
IF _tonTimeout.Q THEN
|
|
_tonTimeout(IN := FALSE);
|
|
_eCmd := E_PackMLCmd.ABORT;
|
|
END_IF
|
|
|
|
// Robot has error
|
|
IF _dwErrorBits <> 0 THEN
|
|
_tonTimeout(IN := FALSE);
|
|
_eCmd := E_PackMLCmd.ABORT;
|
|
END_IF
|
|
END_CASE
|
|
|
|
|
|
// Wrong job read back from robot -> Abort
|
|
// IF _awJobStatesFromRobot[0] <> 0 AND _awJobStatesFromRobot[0] <> E_KukaRobot_JobNumberRobot.WARMUP THEN
|
|
// _eCmd := E_PackMLCmd.ABORT;
|
|
// END_IF
|
|
//
|
|
]]></ST>
|
|
</Implementation>
|
|
</Method>
|
|
<Method Name="M_Stopped" Id="{50813767-4bd7-4147-b5dc-1bbd18ae1555}">
|
|
<Declaration><![CDATA[METHOD M_Stopped
|
|
]]></Declaration>
|
|
<Implementation>
|
|
<ST><![CDATA[_uCtrl.stCtrl.bConfMess := 0;
|
|
_uCtrl.stCtrl.bEnableAxes := 0;
|
|
_uCtrl.stCtrl.bNotDisableAxes := 1;
|
|
_uCtrl.stCtrl.bExtStart := 0;
|
|
|
|
_uJobs.stJobs.wJobNrForRobot := E_KukaRobot_JobNumberRobot.NO_JOB;
|
|
_ePlcJob := E_KukaRobot_JobNumerPLC.NO_JOB;]]></ST>
|
|
</Implementation>
|
|
</Method>
|
|
<Method Name="M_Suspended" Id="{c2db015c-1cd0-491a-8ce6-6c0bffba7631}">
|
|
<Declaration><![CDATA[METHOD M_Suspended
|
|
]]></Declaration>
|
|
<Implementation>
|
|
<ST><![CDATA[]]></ST>
|
|
</Implementation>
|
|
</Method>
|
|
<Method Name="M_Suspending" Id="{5b0e5227-d5bb-40c6-a1b8-8e81b852b9b9}">
|
|
<Declaration><![CDATA[METHOD M_Suspending
|
|
]]></Declaration>
|
|
<Implementation>
|
|
<ST><![CDATA[]]></ST>
|
|
</Implementation>
|
|
</Method>
|
|
<Method Name="M_Unholding" Id="{786f4949-9630-4ace-9e30-65bea4e87cb2}">
|
|
<Declaration><![CDATA[METHOD M_Unholding
|
|
]]></Declaration>
|
|
<Implementation>
|
|
<ST><![CDATA[]]></ST>
|
|
</Implementation>
|
|
</Method>
|
|
<Method Name="M_Unsuspending" Id="{d7ce33fd-cd94-44f1-9289-25a3ab3829eb}">
|
|
<Declaration><![CDATA[METHOD M_Unsuspending
|
|
]]></Declaration>
|
|
<Implementation>
|
|
<ST><![CDATA[]]></ST>
|
|
</Implementation>
|
|
</Method>
|
|
</POU>
|
|
</TcPlcObject> |