First bigger step to automation
- Aligned X and Y NC Axis with aligner camera coordination system - Added robot plc jobs and feedback - Began Meca500 robot interface - Changed hotplate control to slow PWM - PackML statemachine now starts in aborted state - Fixed StateML start method
This commit is contained in:
@@ -9,6 +9,7 @@ VAR
|
||||
_fbEtcher2 : FB_Etcher;
|
||||
_fbTrayFeederInput : FB_TrayFeeder(sIPAddr := '192.168.1.10', udiPort := 5000);
|
||||
_fbHeatCoolPlates : FB_HeatCoolPlates;
|
||||
_fbAligner : FB_Aligner;
|
||||
|
||||
_xConfirmAlarms : BOOL;
|
||||
|
||||
@@ -17,6 +18,7 @@ VAR
|
||||
_stRobotAdmin : ST_PMLa;
|
||||
|
||||
_stRobotJobParams : ST_KukaRobot_JobParams;
|
||||
_stUnitFeedbacks : ST_KukaRobot_UnitFeedbacks;
|
||||
|
||||
_stCamResult AT %I* : ST_TrayFeederCamPosData;
|
||||
|
||||
@@ -31,9 +33,13 @@ VAR
|
||||
_xUnhold : BOOL;
|
||||
_xStop : BOOL;
|
||||
|
||||
_xStartCycle : BOOL;
|
||||
|
||||
_xStartTrigger : BOOL;
|
||||
_tofTriggerTime : TOF := (PT := T#1S);
|
||||
_xTriggerCamera AT %Q* : BOOL;
|
||||
|
||||
_iState : INT;
|
||||
END_VAR
|
||||
]]></Declaration>
|
||||
<Implementation>
|
||||
@@ -73,11 +79,15 @@ IF _rtStopRobotFromSafety.Q THEN
|
||||
_stRobotCmd.xCmdChangeRequest := TRUE;
|
||||
END_IF
|
||||
|
||||
_stUnitFeedbacks.xDoorEtcher1Open := _fbEtcher1.xDoorOpen;
|
||||
_stUnitFeedbacks.xDoorEtcher2Open := _fbEtcher2.xDoorOpen;
|
||||
|
||||
_fbRobot(
|
||||
stCommand:= _stRobotCmd,
|
||||
stJobParams := _stRobotJobParams,
|
||||
stUnitFeedbacks := _stUnitFeedbacks,
|
||||
xReleaseAlarms:= TRUE,
|
||||
xConfirmAlarms:= _xConfirmAlarms,
|
||||
xConfirmAlarms:= GVL_SCADA.xErrAck,
|
||||
stStatus => _stRobotStatus,
|
||||
stAdmin => _stRobotAdmin,
|
||||
xReady=> ,
|
||||
@@ -89,22 +99,83 @@ END_IF
|
||||
|
||||
_fbHVTester(xOpenChambers:= GVL_SCADA.xOpenAllChambers);
|
||||
|
||||
_fbEtcher1(xOpenDoor:= GVL_SCADA.xOpenAllChambers, xConfirmAlarms := GVL_SCADA.xErrAck);
|
||||
_fbEtcher2(xOpenDoor:= GVL_SCADA.xOpenAllChambers, xConfirmAlarms := GVL_SCADA.xErrAck);
|
||||
_fbEtcher1(
|
||||
xOpenDoor:= GVL_SCADA.xOpenAllChambers,
|
||||
xOpenChuckClamp := GVL_SCADA.xOpenChuckClampEtcher1,
|
||||
xConfirmAlarms := GVL_SCADA.xErrAck);
|
||||
|
||||
_fbEtcher2(
|
||||
xOpenDoor:= GVL_SCADA.xOpenAllChambers,
|
||||
xOpenChuckClamp := GVL_SCADA.xOpenChuckClampEtcher2,
|
||||
xConfirmAlarms := GVL_SCADA.xErrAck);
|
||||
|
||||
// _fbTrayFeederInput(
|
||||
// stCommand:= ,
|
||||
// xConfirmAlarms := GVL_SCADA.xErrAck,
|
||||
// stStatus=> GVL_SCADA.stTrayFeederInputState,
|
||||
// stAdmin=> GVL_SCADA.stTRayFeederInputAdmin);
|
||||
_fbTrayFeederInput(
|
||||
stCommand:= ,
|
||||
xConfirmAlarms := GVL_SCADA.xErrAck,
|
||||
stStatus=> GVL_SCADA.stTrayFeederInputState,
|
||||
stAdmin=> GVL_SCADA.stTRayFeederInputAdmin);
|
||||
|
||||
_fbHeatCoolPlates(xConfirmAlarms:= GVL_SCADA.xErrAck);
|
||||
|
||||
_fbAligner(stCommand:= , stStatus=> , stAdmin=> , xConfirmAlarms:= GVL_SCADA.xErrAck);
|
||||
|
||||
|
||||
// Call safety program
|
||||
PRG_Safety(
|
||||
xConfirmAlarms := GVL_SCADA.xErrAck,
|
||||
xRobotStopped := (_stRobotStatus.eStateCurrent <> E_PackMLState.EXECUTE));
|
||||
|
||||
// Handle robot cmds
|
||||
M_HandleRobotCmd();
|
||||
|
||||
// Main state machine
|
||||
CASE _iState OF
|
||||
0: // Idle
|
||||
IF _xStartCycle THEN
|
||||
_xStartCycle := FALSE;
|
||||
|
||||
// Only start with robot in idle
|
||||
IF _stRobotStatus.eStateCurrent = E_PackMLState.IDLE THEN
|
||||
_iState := 10;
|
||||
END_IF
|
||||
END_IF
|
||||
|
||||
// Grab from position 1
|
||||
10:
|
||||
// Prepare robot command
|
||||
_stRobotJobParams.eJob := E_KukaRobot_JobNumberRobot.GET_FROM_INPUT;
|
||||
_stRobotJobParams.rPosX := 88.0;
|
||||
_stRobotJobParams.rPosY := 79.0;
|
||||
_stRobotJobParams.byGripperSide := 3;
|
||||
|
||||
_stRobotCmd.eCntrlCmd := E_PackMLCmd.START;
|
||||
_stRobotCmd.xCmdChangeRequest := TRUE;
|
||||
|
||||
_iState := 20;
|
||||
|
||||
// Wait for Robot to be done
|
||||
20:
|
||||
IF (_stRobotStatus.eStateCurrent = E_PackMLState.IDLE) THEN
|
||||
_iState := 30;
|
||||
END_IF
|
||||
|
||||
// Job aborted
|
||||
IF (_stRobotStatus.eStateCurrent = E_PackMLState.ABORTED) OR (_stRobotStatus.eStateCurrent = E_PackMLState.STOPPED) THEN
|
||||
_iState := 900;
|
||||
END_IF
|
||||
|
||||
// Put part onto aligner
|
||||
30:
|
||||
_stRobotJobParams.eJob := E_KukaRobot_JobNumberRobot.PUT_INTO_ALIGNER;
|
||||
|
||||
_stRobotCmd.eCntrlCmd := E_PackMLCmd.START;
|
||||
_stRobotCmd.xCmdChangeRequest := TRUE;
|
||||
|
||||
_iState := 40;
|
||||
|
||||
// Align blank
|
||||
40:
|
||||
END_CASE
|
||||
|
||||
// =====
|
||||
// DEBUG
|
||||
@@ -123,5 +194,131 @@ IF _xConfirmAlarms THEN
|
||||
_xConfirmAlarms := FALSE;
|
||||
END_IF]]></ST>
|
||||
</Implementation>
|
||||
<Method Name="M_HandleRobotCmd" Id="{14934aaa-b667-4ddb-915d-1d84e8eb799d}">
|
||||
<Declaration><![CDATA[METHOD PRIVATE M_HandleRobotCmd
|
||||
VAR_INST
|
||||
_iState : INT;
|
||||
_tonTimeout : TON;
|
||||
END_VAR
|
||||
]]></Declaration>
|
||||
<Implementation>
|
||||
<ST><![CDATA[// Handle robot commands to plc
|
||||
CASE _iState OF
|
||||
// Wait for command
|
||||
0:
|
||||
_fbRobot.xAckPLCCmd := FALSE;
|
||||
|
||||
IF _fbRobot.xNewCmdRequested THEN
|
||||
_tonTimeout(IN := FALSE);
|
||||
_fbRobot.xAckPLCCmd := FALSE;
|
||||
_fbRobot.xPLCJobFailed := FALSE;
|
||||
_iState := 10;
|
||||
END_IF
|
||||
|
||||
// Do command
|
||||
10:
|
||||
CASE _fbRobot.eCmdFromRobot OF
|
||||
E_KukaRobot_JobNumerPLC.ENABLE_VACUUM_ALIGNER:
|
||||
_fbAligner.xEnableVacuum := TRUE;
|
||||
|
||||
// Check for timeout
|
||||
_tonTimeout(IN := TRUE, PT := T#10S);
|
||||
IF _tonTimeout.Q THEN
|
||||
_fbRobot.xAckPLCCmd := TRUE;
|
||||
_fbRobot.xPLCJobFailed := TRUE;
|
||||
_iState := 0;
|
||||
END_IF
|
||||
|
||||
IF _fbAligner.xVacuumEnabled THEN
|
||||
_fbRobot.xAckPLCCmd := TRUE;
|
||||
_iState := 0;
|
||||
END_IF
|
||||
|
||||
E_KukaRobot_JobNumerPLC.DISABLE_VACUUM_ALIGNER:
|
||||
_fbAligner.xEnableVacuum := FALSE;
|
||||
|
||||
// Check for timeout
|
||||
_tonTimeout(IN := TRUE, PT := T#10S);
|
||||
IF _tonTimeout.Q THEN
|
||||
_fbRobot.xAckPLCCmd := TRUE;
|
||||
_fbRobot.xPLCJobFailed := TRUE;
|
||||
_iState := 0;
|
||||
END_IF
|
||||
|
||||
IF (NOT _fbAligner.xVacuumEnabled) THEN
|
||||
_fbRobot.xAckPLCCmd := TRUE;
|
||||
_iState := 0;
|
||||
END_IF
|
||||
|
||||
E_KukaRobot_JobNumerPLC.ENABLE_VACUUM_ETCHER1:
|
||||
_fbEtcher1.xEnableVacuum := TRUE;
|
||||
|
||||
// Check for timeout
|
||||
_tonTimeout(IN := TRUE, PT := T#20S);
|
||||
IF _tonTimeout.Q THEN
|
||||
_fbRobot.xAckPLCCmd := TRUE;
|
||||
_fbRobot.xPLCJobFailed := TRUE;
|
||||
_iState := 0;
|
||||
END_IF
|
||||
|
||||
IF _fbEtcher1.xVacuumEnabled THEN
|
||||
_fbRobot.xAckPLCCmd := TRUE;
|
||||
_iState := 0;
|
||||
END_IF
|
||||
|
||||
E_KukaRobot_JobNumerPLC.DISABLE_VACUUM_ETCHER1:
|
||||
_fbEtcher1.xEnableVacuum := FALSE;
|
||||
|
||||
// Check for timeout
|
||||
_tonTimeout(IN := TRUE, PT := T#20S);
|
||||
IF _tonTimeout.Q THEN
|
||||
_fbRobot.xAckPLCCmd := TRUE;
|
||||
_fbRobot.xPLCJobFailed := TRUE;
|
||||
_iState := 0;
|
||||
END_IF
|
||||
|
||||
IF (NOT _fbEtcher1.xVacuumEnabled) THEN
|
||||
_fbRobot.xAckPLCCmd := TRUE;
|
||||
_iState := 0;
|
||||
END_IF
|
||||
|
||||
E_KukaRobot_JobNumerPLC.ENABLE_VACUUM_ETCHER2:
|
||||
_fbEtcher2.xEnableVacuum := TRUE;
|
||||
|
||||
// Check for timeout
|
||||
_tonTimeout(IN := TRUE, PT := T#20S);
|
||||
IF _tonTimeout.Q THEN
|
||||
_fbRobot.xAckPLCCmd := TRUE;
|
||||
_fbRobot.xPLCJobFailed := TRUE;
|
||||
_iState := 0;
|
||||
END_IF
|
||||
|
||||
IF _fbEtcher2.xVacuumEnabled THEN
|
||||
_fbRobot.xAckPLCCmd := TRUE;
|
||||
_iState := 0;
|
||||
END_IF
|
||||
|
||||
E_KukaRobot_JobNumerPLC.DISABLE_VACUUM_ETCHER2:
|
||||
_fbEtcher2.xEnableVacuum := FALSE;
|
||||
|
||||
// Check for timeout
|
||||
_tonTimeout(IN := TRUE, PT := T#20S);
|
||||
IF _tonTimeout.Q THEN
|
||||
_fbRobot.xAckPLCCmd := TRUE;
|
||||
_fbRobot.xPLCJobFailed := TRUE;
|
||||
_iState := 0;
|
||||
END_IF
|
||||
|
||||
IF (NOT _fbEtcher2.xVacuumEnabled) THEN
|
||||
_fbRobot.xAckPLCCmd := TRUE;
|
||||
_iState := 0;
|
||||
END_IF
|
||||
|
||||
ELSE
|
||||
_iState := 90;
|
||||
END_CASE
|
||||
END_CASE]]></ST>
|
||||
</Implementation>
|
||||
</Method>
|
||||
</POU>
|
||||
</TcPlcObject>
|
||||
Reference in New Issue
Block a user