303 lines
7.1 KiB
XML
303 lines
7.1 KiB
XML
<?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 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[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> |