More Meca implementation
This commit is contained in:
@@ -76,6 +76,8 @@ VAR
|
||||
// Actuators
|
||||
// =========
|
||||
|
||||
_fbRobot : FB_Mecademics;
|
||||
|
||||
xDisableVacuum AT %Q* : BOOL;
|
||||
END_VAR
|
||||
]]></Declaration>
|
||||
@@ -132,6 +134,8 @@ _fbEjectBack(
|
||||
|
||||
xDisableVacuum := (NOT xEnableVacuum);
|
||||
|
||||
_fbRobot(stPackMLHMIInterface := stHMIInterface.stMecaCmds);
|
||||
|
||||
// Call base sm
|
||||
SUPER^(stPackMLHMIInterface := stHMIInterface.stStationCmds);
|
||||
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
<TcPlcObject Version="1.1.0.1">
|
||||
<GVL Name="GVL_ETCHER" Id="{ef1ba925-5452-40f1-b847-57ee34065909}">
|
||||
<Declaration><![CDATA[{attribute 'qualified_only'}
|
||||
VAR_GLOBAL
|
||||
VAR_GLOBAL CONSTANT
|
||||
MAX_ROBOT_POS : UINT := 10;
|
||||
END_VAR]]></Declaration>
|
||||
</GVL>
|
||||
|
||||
@@ -53,7 +53,7 @@ TYPE E_Meca_Cmds :
|
||||
SET_TOOL_SPHERE := 155,
|
||||
SET_CALIBRATION_CFG := 156,
|
||||
REBOOT_ROBOT := 200
|
||||
);
|
||||
)UDINT := NO_MOVEMENT;
|
||||
END_TYPE
|
||||
]]></Declaration>
|
||||
</DUT>
|
||||
|
||||
@@ -0,0 +1,14 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<TcPlcObject Version="1.1.0.1">
|
||||
<DUT Name="ST_Meca_Configurations" Id="{d428df95-5825-4c78-9c82-db07b6f06b2a}">
|
||||
<Declaration><![CDATA[TYPE ST_Meca_Configurations :
|
||||
STRUCT
|
||||
siShoulder : SINT;
|
||||
siElbow : SINT;
|
||||
siWrist : SINT;
|
||||
siTurn : SINT;
|
||||
END_STRUCT
|
||||
END_TYPE
|
||||
]]></Declaration>
|
||||
</DUT>
|
||||
</TcPlcObject>
|
||||
@@ -0,0 +1,17 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<TcPlcObject Version="1.1.0.1">
|
||||
<DUT Name="ST_Meca_DynamicData" Id="{eb440853-1144-466e-801d-c83f27997752}">
|
||||
<Declaration><![CDATA[TYPE ST_Meca_DynamicData :
|
||||
STRUCT
|
||||
udiDynamic_Type : UDINT;
|
||||
rValue_0 : REAL;
|
||||
rValue_1 : REAL;
|
||||
rValue_2 : REAL;
|
||||
rValue_3 : REAL;
|
||||
rValue_4 : REAL;
|
||||
rValue_5 : REAL;
|
||||
END_STRUCT
|
||||
END_TYPE
|
||||
]]></Declaration>
|
||||
</DUT>
|
||||
</TcPlcObject>
|
||||
@@ -0,0 +1,16 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<TcPlcObject Version="1.1.0.1">
|
||||
<DUT Name="ST_Meca_EndEffectorPose" Id="{550369ad-a02a-4994-845f-0e5bcd8c28d2}">
|
||||
<Declaration><![CDATA[TYPE ST_Meca_EndEffectorPose :
|
||||
STRUCT
|
||||
rSubIndex_001 : REAL;
|
||||
rSubIndex_002 : REAL;
|
||||
rSubIndex_003 : REAL;
|
||||
rSubIndex_004 : REAL;
|
||||
rSubIndex_005 : REAL;
|
||||
rSubIndex_006 : REAL;
|
||||
END_STRUCT
|
||||
END_TYPE
|
||||
]]></Declaration>
|
||||
</DUT>
|
||||
</TcPlcObject>
|
||||
@@ -0,0 +1,13 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<TcPlcObject Version="1.1.0.1">
|
||||
<DUT Name="ST_Meca_RobotTimestamp" Id="{8c6943a9-eb48-4002-9676-3b975565fe21}">
|
||||
<Declaration><![CDATA[TYPE ST_Meca_RobotTimestamp :
|
||||
STRUCT
|
||||
udiTimestamp_Seconds : UDINT;
|
||||
udiTimestamp_Microseconds : UDINT;
|
||||
udiDynamic_Data_Cycles : UDINT;
|
||||
END_STRUCT
|
||||
END_TYPE
|
||||
]]></Declaration>
|
||||
</DUT>
|
||||
</TcPlcObject>
|
||||
@@ -0,0 +1,95 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<TcPlcObject Version="1.1.0.1">
|
||||
<DUT Name="ST_Meca_SafetyStatus" Id="{87f1a6a3-00a5-4cfe-890e-38e5886ffb63}">
|
||||
<Declaration><![CDATA[TYPE ST_Meca_SafetyStatus :
|
||||
STRUCT
|
||||
bEstop : BIT;
|
||||
bUnused2 : BIT;
|
||||
bPStop2 : BIT;
|
||||
bUnused4 : BIT;
|
||||
bUnused5 : BIT;
|
||||
bUnused6 : BIT;
|
||||
bUnused7 : BIT;
|
||||
bReboot : BIT;
|
||||
bUnused9 : BIT;
|
||||
bUnused10 : BIT;
|
||||
bConnection_Dropped : BIT;
|
||||
bUnused12 : BIT;
|
||||
bUnused13 : BIT;
|
||||
bUnused14 : BIT;
|
||||
bUnused15 : BIT;
|
||||
bUnused16 : BIT;
|
||||
bUnused17 : BIT;
|
||||
bUnused18 : BIT;
|
||||
bUnused19 : BIT;
|
||||
bUnused20 : BIT;
|
||||
bUnused21 : BIT;
|
||||
bUnused22 : BIT;
|
||||
bUnused23 : BIT;
|
||||
bUnused24 : BIT;
|
||||
bUnused25 : BIT;
|
||||
bUnused26 : BIT;
|
||||
bUnused27 : BIT;
|
||||
bUnused28 : BIT;
|
||||
bUnused29 : BIT;
|
||||
bUnused30 : BIT;
|
||||
bUnused31 : BIT;
|
||||
bUnused32 : BIT;
|
||||
bEstop_Resettable : BIT;
|
||||
bUnused34 : BIT;
|
||||
bPStop2_Resettable : BIT;
|
||||
bUnused36 : BIT;
|
||||
bUnused37 : BIT;
|
||||
bUnused38 : BIT;
|
||||
bUnused39 : BIT;
|
||||
bReboot_Resettable : BIT;
|
||||
bUnused41 : BIT;
|
||||
bUnused42 : BIT;
|
||||
bConnection_Dropped_Resettable : BIT;
|
||||
bUnused44 : BIT;
|
||||
bUnused45 : BIT;
|
||||
bUnused46 : BIT;
|
||||
bUnused47 : BIT;
|
||||
bUnused48 : BIT;
|
||||
bUnused49 : BIT;
|
||||
bUnused50 : BIT;
|
||||
bUnused51 : BIT;
|
||||
bUnused52 : BIT;
|
||||
bUnused53 : BIT;
|
||||
bUnused54 : BIT;
|
||||
bUnused55 : BIT;
|
||||
bUnused56 : BIT;
|
||||
bUnused57 : BIT;
|
||||
bUnused58 : BIT;
|
||||
bUnused59 : BIT;
|
||||
bUnused60 : BIT;
|
||||
bUnused61 : BIT;
|
||||
bUnused62 : BIT;
|
||||
bUnused63 : BIT;
|
||||
bUnused64 : BIT;
|
||||
bUnused65 : USINT;
|
||||
bReset_Ready : BIT;
|
||||
bVmotor_on : BIT;
|
||||
bUnused68 : BIT;
|
||||
bUnused69 : BIT;
|
||||
bUnused70 : BIT;
|
||||
bUnused71 : BIT;
|
||||
bUnused72 : BIT;
|
||||
bUnused73 : BIT;
|
||||
bUnused74 : BIT;
|
||||
bUnused75 : BIT;
|
||||
bUnused76 : BIT;
|
||||
bUnused77 : BIT;
|
||||
bUnused78 : BIT;
|
||||
bUnused79 : BIT;
|
||||
{attribute 'hide'}
|
||||
breserved1 : BIT;
|
||||
{attribute 'hide'}
|
||||
breserved2 : BIT;
|
||||
{attribute 'hide'}
|
||||
breserved3 : USINT;
|
||||
END_STRUCT
|
||||
END_TYPE
|
||||
]]></Declaration>
|
||||
</DUT>
|
||||
</TcPlcObject>
|
||||
@@ -1,8 +1,8 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<TcPlcObject Version="1.1.0.1">
|
||||
<DUT Name="ST_Meca_JointSet" Id="{11c9caa0-793d-42a4-a6d6-9b5ea59d2923}">
|
||||
<DUT Name="ST_Meca_SubIndex" Id="{11c9caa0-793d-42a4-a6d6-9b5ea59d2923}">
|
||||
<Declaration><![CDATA[{attribute 'pack_mode' := '1'}
|
||||
TYPE ST_Meca_JointSet :
|
||||
TYPE ST_Meca_SubIndex :
|
||||
STRUCT
|
||||
rSubIndex001 : REAL;
|
||||
rSubIndex002 : REAL;
|
||||
@@ -0,0 +1,12 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<TcPlcObject Version="1.1.0.1">
|
||||
<DUT Name="ST_Meca_BreaksControl" Id="{d002e7e2-c385-4270-8e84-4eabb3a35679}">
|
||||
<Declaration><![CDATA[TYPE ST_Meca_BreaksControl :
|
||||
STRUCT
|
||||
bBrakesControlAllowed : BIT;
|
||||
bBrakesEngaged : BIT;
|
||||
END_STRUCT
|
||||
END_TYPE
|
||||
]]></Declaration>
|
||||
</DUT>
|
||||
</TcPlcObject>
|
||||
@@ -0,0 +1,15 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<TcPlcObject Version="1.1.0.1">
|
||||
<DUT Name="ST_Meca_MotionControl" Id="{74b560dc-7b24-4a46-bcf7-f3928a356b76}">
|
||||
<Declaration><![CDATA[TYPE ST_Meca_MotionControl :
|
||||
STRUCT
|
||||
Move_ID : UINT;
|
||||
SetPoint : BIT;
|
||||
Pause : BIT;
|
||||
Clear_Move : BIT;
|
||||
Resume_Motion : BIT;
|
||||
END_STRUCT
|
||||
END_TYPE
|
||||
]]></Declaration>
|
||||
</DUT>
|
||||
</TcPlcObject>
|
||||
@@ -0,0 +1,17 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<TcPlcObject Version="1.1.0.1">
|
||||
<DUT Name="ST_Meca_Move" Id="{fb9f8b41-eddc-4ca4-9e77-c3d7e9282c37}">
|
||||
<Declaration><![CDATA[TYPE ST_Meca_Move :
|
||||
STRUCT
|
||||
udiMove_Command : UDINT;
|
||||
rSubIndex_001 : REAL;
|
||||
rSubIndex_002 : REAL;
|
||||
rSubIndex_003 : REAL;
|
||||
rSubIndex_004 : REAL;
|
||||
rSubIndex_005 : REAL;
|
||||
rSubIndex_006 : REAL;
|
||||
END_STRUCT
|
||||
END_TYPE
|
||||
]]></Declaration>
|
||||
</DUT>
|
||||
</TcPlcObject>
|
||||
@@ -0,0 +1,21 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<TcPlcObject Version="1.1.0.1">
|
||||
<DUT Name="ST_Meca_RobotControl" Id="{b7dca174-83b7-4e16-8fce-bddd2eb93964}">
|
||||
<Declaration><![CDATA[TYPE ST_Meca_RobotControl :
|
||||
STRUCT
|
||||
bDeactivate : BIT;
|
||||
bActivate : BIT;
|
||||
bHome : BIT;
|
||||
bReset_Error : BIT;
|
||||
bSim_Mode : BIT;
|
||||
bRecovery_Mode : BIT;
|
||||
bDisableEtherCAT : BIT;
|
||||
{attribute 'hide'}
|
||||
_reserved1 : BIT;
|
||||
{attribute 'hide'}
|
||||
_reserved2 : ARRAY [1..3] OF USINT;
|
||||
END_STRUCT
|
||||
END_TYPE
|
||||
]]></Declaration>
|
||||
</DUT>
|
||||
</TcPlcObject>
|
||||
23
PLC/01_Stationen/Etcher/Mecademics/DUTs/ST_Meca_Inputs.TcDUT
Normal file
23
PLC/01_Stationen/Etcher/Mecademics/DUTs/ST_Meca_Inputs.TcDUT
Normal file
@@ -0,0 +1,23 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<TcPlcObject Version="1.1.0.1">
|
||||
<DUT Name="ST_Meca_Inputs" Id="{aee4d747-0300-4e1c-963b-4297e692d221}">
|
||||
<Declaration><![CDATA[TYPE ST_Meca_Inputs :
|
||||
STRUCT
|
||||
stRobotStatus : ST_Meca_RobotStatus;
|
||||
stMotionStatus : ST_Meca_MotionStatus;
|
||||
stJointSet : ST_Meca_SubIndex;
|
||||
stEndEffectorPose : ST_Meca_EndEffectorPose;
|
||||
stConfigurations : ST_Meca_Configurations;
|
||||
stWRF : ST_Meca_SubIndex;
|
||||
stTRF : ST_Meca_SubIndex;
|
||||
stRobotTimestamp : ST_Meca_RobotTimestamp;
|
||||
stSafetyStatus : ST_Meca_SafetyStatus;
|
||||
stDynamicData0 : ST_Meca_DynamicData;
|
||||
stDynamicData1 : ST_Meca_DynamicData;
|
||||
stDynamicData2 : ST_Meca_DynamicData;
|
||||
stDynamicData3 : ST_Meca_DynamicData;
|
||||
END_STRUCT
|
||||
END_TYPE
|
||||
]]></Declaration>
|
||||
</DUT>
|
||||
</TcPlcObject>
|
||||
@@ -0,0 +1,19 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<TcPlcObject Version="1.1.0.1">
|
||||
<DUT Name="ST_Meca_Outputs" Id="{4c92256b-96a9-4346-9b9b-5d7a337cd273}">
|
||||
<Declaration><![CDATA[TYPE ST_Meca_Outputs :
|
||||
STRUCT
|
||||
stRobotControl : ST_Meca_RobotControl;
|
||||
stMotionControl : ST_Meca_MotionControl;
|
||||
stMove : ST_Meca_Move;
|
||||
udiHostTime : UDINT;
|
||||
stBrakesControl : ST_Meca_BreaksControl;
|
||||
udiDynamicDataCfg0 : UDINT;
|
||||
udiDynamicDataCfg1 : UDINT;
|
||||
udiDynamicDataCfg2 : UDINT;
|
||||
udiDynamicDataCfg3 : UDINT;
|
||||
END_STRUCT
|
||||
END_TYPE
|
||||
]]></Declaration>
|
||||
</DUT>
|
||||
</TcPlcObject>
|
||||
@@ -1,16 +1,303 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<TcPlcObject Version="1.1.0.1">
|
||||
<POU Name="FB_Mecademics" Id="{c81cf0ae-3f01-4427-918b-0d3e1c93ae01}" SpecialFunc="None">
|
||||
<Declaration><![CDATA[FUNCTION_BLOCK FB_Mecademics
|
||||
<Declaration><![CDATA[FUNCTION_BLOCK FB_Mecademics EXTENDS FB_PackMLGeneric
|
||||
VAR_INPUT
|
||||
END_VAR
|
||||
VAR_OUTPUT
|
||||
xBusy : BOOL;
|
||||
xDone : BOOL;
|
||||
xError : BOOL;
|
||||
END_VAR
|
||||
VAR
|
||||
stRobotInputs AT %I* : ST_Meca_Inputs;
|
||||
stRobotOutputs AT %Q* : ST_Meca_Outputs;
|
||||
|
||||
// Safety reset output
|
||||
xReset AT %Q* : BOOL;
|
||||
|
||||
_fbResetPulse : TP;
|
||||
|
||||
// Current move id
|
||||
_uiMoveId : UINT := 1;
|
||||
|
||||
// Debug
|
||||
_xTest : BOOL;
|
||||
_xDone : BOOL;
|
||||
_xError : BOOL;
|
||||
_iDS : INT;
|
||||
END_VAR
|
||||
]]></Declaration>
|
||||
<Implementation>
|
||||
<ST><![CDATA[]]></ST>
|
||||
<ST><![CDATA[SUPER^(stPackMLHMIInterface := stPackMLHMIInterface);
|
||||
|
||||
|
||||
CASE _iDS OF
|
||||
0:
|
||||
IF _xTest THEN
|
||||
_xTest := FALSE;
|
||||
_iDS := 10;
|
||||
END_IF
|
||||
|
||||
10:
|
||||
M_ExecuteCmd(
|
||||
eCmd:= E_Meca_Cmds.MOVE_LIN,
|
||||
rSI1:= 80,
|
||||
rSI2:= 80,
|
||||
rSI3:= 10,
|
||||
rSI4:= 0,
|
||||
rSI5:= 0,
|
||||
rSI6:= 0,
|
||||
xDone=> _xDone,
|
||||
xError=> _xError);
|
||||
|
||||
IF _xDone OR _xError THEN
|
||||
_iDS := 0;
|
||||
END_IF
|
||||
|
||||
END_CASE]]></ST>
|
||||
</Implementation>
|
||||
<Method Name="FB_init" Id="{b39b41a5-5e30-4480-88f3-9748b615d950}">
|
||||
<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.xSuspededDisabled := TRUE;
|
||||
_stSMConfig.xHeldDisabled := TRUE;
|
||||
_stSMConfig.xCompletedDisabled := TRUE;
|
||||
_stSMConfig.xClearingDisabled := TRUE;
|
||||
]]></ST>
|
||||
</Implementation>
|
||||
</Method>
|
||||
<Method Name="M_ClearCmd" Id="{3363b63b-d0d9-4750-b0b8-c2b0abdc2690}">
|
||||
<Declaration><![CDATA[METHOD PRIVATE M_ClearCmd
|
||||
VAR_INPUT
|
||||
END_VAR
|
||||
]]></Declaration>
|
||||
<Implementation>
|
||||
<ST><![CDATA[stRobotOutputs.stMove.udiMove_Command := E_Meca_Cmds.NO_MOVEMENT;
|
||||
stRobotOutputs.stMove.rSubIndex_001 := 0;
|
||||
stRobotOutputs.stMove.rSubIndex_002 := 0;
|
||||
stRobotOutputs.stMove.rSubIndex_003 := 0;
|
||||
stRobotOutputs.stMove.rSubIndex_004 := 0;
|
||||
stRobotOutputs.stMove.rSubIndex_005 := 0;
|
||||
stRobotOutputs.stMove.rSubIndex_006 := 0;
|
||||
|
||||
stRobotOutputs.stMotionControl.Move_ID := 0;
|
||||
stRobotOutputs.stMotionControl.SetPoint := 0;]]></ST>
|
||||
</Implementation>
|
||||
</Method>
|
||||
<Method Name="M_Execute" Id="{b1f00bfa-a0a0-4a86-98a4-0a9242f7788f}">
|
||||
<Declaration><![CDATA[METHOD PROTECTED M_Execute
|
||||
]]></Declaration>
|
||||
<Implementation>
|
||||
<ST><![CDATA[CASE _iSSM OF
|
||||
// Idle no command
|
||||
0:
|
||||
// Wait for robot command execute trigger
|
||||
|
||||
END_CASE]]></ST>
|
||||
</Implementation>
|
||||
</Method>
|
||||
<Method Name="M_ExecuteCmd" Id="{5a17ad67-b9f0-443e-b4f3-cbcd71a5ce3e}">
|
||||
<Declaration><![CDATA[METHOD PRIVATE M_ExecuteCmd
|
||||
VAR_INPUT
|
||||
eCmd : E_Meca_Cmds;
|
||||
rSI1 : REAL := 0;
|
||||
rSI2 : REAL := 0;
|
||||
rSI3 : REAL := 0;
|
||||
rSI4 : REAL := 0;
|
||||
rSI5 : REAL := 0;
|
||||
rSI6 : REAL := 0;
|
||||
END_VAR
|
||||
VAR_OUTPUT
|
||||
xDone : BOOL;
|
||||
xError : BOOL;
|
||||
END_VAR
|
||||
VAR_INST
|
||||
_iState : INT;
|
||||
END_VAR]]></Declaration>
|
||||
<Implementation>
|
||||
<ST><![CDATA[CASE _iState OF
|
||||
0:
|
||||
stRobotOutputs.stMove.udiMove_Command := eCmd;
|
||||
stRobotOutputs.stMove.rSubIndex_001 := rSI1;
|
||||
stRobotOutputs.stMove.rSubIndex_002 := rSI2;
|
||||
stRobotOutputs.stMove.rSubIndex_003 := rSI3;
|
||||
stRobotOutputs.stMove.rSubIndex_004 := rSI4;
|
||||
stRobotOutputs.stMove.rSubIndex_005 := rSI5;
|
||||
stRobotOutputs.stMove.rSubIndex_006 := rSI6;
|
||||
|
||||
stRobotOutputs.stMotionControl.Move_ID := _uiMoveId;
|
||||
stRobotOutputs.stMotionControl.SetPoint := 1;
|
||||
xDone := FALSE;
|
||||
xError := FALSE;
|
||||
_iState := 10;
|
||||
|
||||
10:
|
||||
IF stRobotInputs.stMotionStatus.uiMoveID = _uiMoveId THEN
|
||||
_uiMoveId := _uiMoveId + 1;
|
||||
M_ClearCmd();
|
||||
xDone := TRUE;
|
||||
_iState := 0;
|
||||
END_IF
|
||||
|
||||
IF stRobotInputs.stRobotStatus.uiError <> 0 THEN
|
||||
_uiMoveId := _uiMoveId + 1;
|
||||
M_ClearCmd();
|
||||
xError := TRUE;
|
||||
_iState := 0;
|
||||
END_IF
|
||||
END_CASE]]></ST>
|
||||
</Implementation>
|
||||
</Method>
|
||||
<Method Name="M_Resetting" Id="{807f1e42-2b9f-4d36-82a0-1e9f3b86646a}">
|
||||
<Declaration><![CDATA[METHOD PROTECTED M_Resetting
|
||||
VAR_INST
|
||||
_tonTimer : TON;
|
||||
xExecDone : BOOL;
|
||||
xExecError : BOOL;
|
||||
END_VAR]]></Declaration>
|
||||
<Implementation>
|
||||
<ST><![CDATA[CASE _iSSM OF
|
||||
0:
|
||||
// Reset E-Stop if required
|
||||
IF stRobotInputs.stSafetyStatus.bEstop THEN
|
||||
// Can we reset the estop?
|
||||
IF stRobotInputs.stSafetyStatus.bEstop_Resettable THEN
|
||||
_iSSM := 10;
|
||||
ELSE
|
||||
_eCmd := E_PackMLCmd.ABORT;
|
||||
END_IF
|
||||
ELSE
|
||||
// Go to activate state
|
||||
_iSSM := 20;
|
||||
END_IF
|
||||
|
||||
// Reset E-Stop
|
||||
10:
|
||||
xReset := TRUE;
|
||||
_tonTimer(IN := TRUE, PT := T#250MS);
|
||||
IF _tonTimer.Q THEN
|
||||
_tonTimer(IN := FALSE);
|
||||
xReset := FALSE;
|
||||
_iSSM := 15;
|
||||
END_IF
|
||||
|
||||
// Check if E-Stop was resetted
|
||||
15:
|
||||
// Timeout safety reset
|
||||
_tonTimer(IN := TRUE, PT := T#10S);
|
||||
IF _tonTimer.Q THEN
|
||||
_tonTimer(IN := FALSE);
|
||||
// TODO trigger error message
|
||||
_eCmd := E_PackMLCmd.ABORT;
|
||||
END_IF
|
||||
|
||||
IF (NOT stRobotInputs.stSafetyStatus.bEstop) THEN
|
||||
_iSSM := 20;
|
||||
END_IF
|
||||
|
||||
|
||||
// Activate robot if needed
|
||||
20:
|
||||
IF (NOT stRobotInputs.stRobotStatus.bActivated) THEN
|
||||
_iSSM := 21;
|
||||
ELSE
|
||||
_iSSM := 30;
|
||||
END_IF
|
||||
|
||||
|
||||
// Activate robot
|
||||
21:
|
||||
stRobotOutputs.stRobotControl.bActivate := TRUE;
|
||||
IF stRobotInputs.stRobotStatus.bActivated THEN
|
||||
stRobotOutputs.stRobotControl.bActivate := FALSE;
|
||||
_iSSM := 30;
|
||||
END_IF
|
||||
|
||||
// Timeout
|
||||
_tonTimer(IN := TRUE, PT := T#10S);
|
||||
IF _tonTimer.Q THEN
|
||||
_tonTimer(IN := FALSE);
|
||||
stRobotOutputs.stRobotControl.bActivate := FALSE;
|
||||
// TODO trigger error message
|
||||
_eCmd := E_PackMLCmd.ABORT;
|
||||
END_IF
|
||||
|
||||
// Check homing
|
||||
30:
|
||||
IF (NOT stRobotInputs.stRobotStatus.bHomed) THEN
|
||||
_iSSM := 31;
|
||||
ELSE
|
||||
_iSSM := 40;
|
||||
END_IF
|
||||
|
||||
// Home robot
|
||||
31:
|
||||
stRobotOutputs.stRobotControl.bHome := TRUE;
|
||||
IF stRobotInputs.stRobotStatus.bHomed THEN
|
||||
stRobotOutputs.stRobotControl.bHome := FALSE;
|
||||
_iSSM := 40;
|
||||
END_IF
|
||||
|
||||
// Timeout
|
||||
_tonTimer(IN := TRUE, PT := T#20S);
|
||||
IF _tonTimer.Q THEN
|
||||
_tonTimer(IN := FALSE);
|
||||
stRobotOutputs.stRobotControl.bHome := FALSE;
|
||||
// TODO trigger error message
|
||||
_eCmd := E_PackMLCmd.ABORT;
|
||||
END_IF
|
||||
|
||||
// Set wrf
|
||||
32:
|
||||
M_ExecuteCmd(E_Meca_Cmds.SET_WRF, 163.5, -34.79, 465, -180, 0, 180, xDone => xExecDone, xError => xExecError);
|
||||
|
||||
IF xExecDone THEN
|
||||
_iSSM := 33;
|
||||
END_IF
|
||||
|
||||
IF xExecError THEN
|
||||
_eCmd := E_PackMLCmd.ABORT;
|
||||
END_IF
|
||||
|
||||
// Set trf
|
||||
33:
|
||||
M_ExecuteCmd(E_Meca_Cmds.SET_TRF, 0, 0, 92, 0, 180, 90, xDone => xExecDone, xError => xExecError);
|
||||
|
||||
IF xExecDone THEN
|
||||
_iSSM := 34;
|
||||
END_IF
|
||||
|
||||
IF xExecError THEN
|
||||
_eCmd := E_PackMLCmd.ABORT;
|
||||
END_IF
|
||||
|
||||
// Move to save position
|
||||
34:
|
||||
M_ExecuteCmd(E_Meca_Cmds.MOVE_LIN, 0, 0, 10, 0, 0, 0, xDone => xExecDone, xError => xExecError);
|
||||
|
||||
IF xExecDone THEN
|
||||
_iSSM := 40;
|
||||
END_IF
|
||||
|
||||
IF xExecError THEN
|
||||
_eCmd := E_PackMLCmd.ABORT;
|
||||
END_IF
|
||||
|
||||
// Robot ready
|
||||
40:
|
||||
M_StateComplete();
|
||||
END_CASE]]></ST>
|
||||
</Implementation>
|
||||
</Method>
|
||||
</POU>
|
||||
</TcPlcObject>
|
||||
@@ -5,6 +5,8 @@
|
||||
STRUCT
|
||||
stStationCmds : ST_HMI_PackML;
|
||||
|
||||
stMecaCmds : ST_HMI_PackML;
|
||||
|
||||
// Vacuum valve
|
||||
stVacuumValve : ST_HMI_VALVE_DATA;
|
||||
|
||||
|
||||
Reference in New Issue
Block a user