分享~~~~~~~~~
(*****************************************************************************
<FB NAME> MC_Power_JEC
<Description> 運転可 / Operation possible
COPYRIGHT (C) 2016 MITSUBISHI ELECTRIC CORPORATION ALL RIGHT RESERVED
*****************************************************************************)
PLS(Enable, bEnablePls);
PLF(Enable, bEnablePlf);
(*========================================================================*
* 有効ON
* Enable ON
*========================================================================*)
IF Enable THEN
(*------------------------------------------------------------------*
* 初期化処理
* Initial processing
*------------------------------------------------------------------*)
IF bEnablePls THEN
Status := FALSE;
Error := FALSE;
ErrorID := 0;
bBusy := FALSE;
bLinkReady := FALSE;
wAxisStatus := 0;
(* 入力ラベル取込 / Input labels transfer *)
wAxisNo := Axis.AxisNo;
MOV(((wAxisNo < 1) OR (16 < wAxisNo)), H1100, ErrorID);
wPosTrqLimit := REAL_TO_INT(Axis.PosTrqLimit *10.0);
MOV((ErrorID = 0) AND ((wPosTrqLimit < 0) OR (10000 < wPosTrqLimit)), H110E, ErrorID);
wNegTrqLimit := REAL_TO_INT(Axis.NegTrqLimit *10.0);
MOV((ErrorID = 0) AND ((wNegTrqLimit < 0) OR (10000 < wNegTrqLimit)), H110E, ErrorID);
(* リンクデバイスのオフセット計算 / Link device offset calculation *)
IF (ErrorID = 0) THEN
wRX3F := ((wAxisNo - 1) * 64) + H003F; (* サイクリック通信準備完了 / Cyclic communication ready command *)
wRWw01 := ((wAxisNo - 1) * 32) + H0001; (* 制御指令 / Control command *)
wRWw14 := ((wAxisNo - 1) * 32) + H0014; (* トルク制限値(正) / Torque limit value(positive) *)
wRWw15 := ((wAxisNo - 1) * 32) + H0015; (* トルク制限値(逆) / Torque limit value(negative) *)
wRWr00 := ((wAxisNo - 1) * 32) + H0000; (* 制御モード表示 / Control mode display *)
wRWr02 := ((wAxisNo - 1) * 32) + H0002; (* 制御ステータス / Control status *)
wRWr0B := ((wAxisNo - 1) * 32) + H000B; (* 制御出力2 / Control output 2 *)
bBusy := TRUE;
END_IF;
END_IF;
(*------------------------------------------------------------------*
* 軸狀態取得 & 実行條件確認
* Axis status acquisition & execution condition check
*------------------------------------------------------------------*)
IF bBusy THEN
(* サイクリック通信狀態確認 / Cyclic communication status check *)
bLinkReady := G_stLinkBasic.bnRX[wRX3F];
IF bLinkReady THEN
uModeDisp := G_stLinkBasic.unRWr[wRWr00];
bZeroVelocity:= ((G_stLinkBasic.unRWr[wRWr0B] AND H0008) = H0008);
(* 軸狀態取得 / Axis status acquisition *)
IF ((G_stLinkBasic.unRWr[wRWr02] AND H0008) = H0008) THEN
wAxisStatus := 1; (* Errorstop *)
ErrorID := H1202;
ELSIF ((G_stLinkBasic.unRWw[wRWw01] AND H0086) = H0002) THEN
wAxisStatus := 2; (* Stopping *)
ELSIF (((G_stLinkBasic.unRWr[wRWr02] AND H000F) = H0000) OR
((G_stLinkBasic.unRWr[wRWr02] AND H000F) = H0001) OR
((G_stLinkBasic.unRWr[wRWr02] AND H000F) = H0003)) THEN
wAxisStatus := 0; (* Disabled *)
ELSE
wAxisStatus := 4; (* Standstill *)
IF ((G_stLinkBasic.unRWw[wRWw01] AND H0100)= H0100) THEN
CASE WORD_TO_INT(uModeDisp) OF
1, (* Profile position mode *)
6: (* Homing mode *)
IF ((G_stLinkBasic.unRWr[wRWr02] AND H0400) = H0000) THEN
wAxisStatus := 5; (* DiscreteMotion *)
END_IF;
3, (* Profile Velocity mode *)
4: (* Profile Torque mode *)
IF NOT bZeroVelocity THEN
wAxisStatus := 5; (* DiscreteMotion *)
END_IF;
ELSE
;
END_CASE;
ELSE
CASE (WORD_TO_INT(uModeDisp)) OF
1:
IF ((G_stLinkBasic.unRWr[wRWr02] AND H0400) = H0000) THEN
wAxisStatus := 5; (* DiscreteMotion *)
END_IF;
3,
4:
wAxisStatus := 6; (* ContinuousMotion *)
6:
IF ((G_stLinkBasic.unRWr[wRWr02] AND H0400) = H0000) THEN
wAxisStatus := 3; (* Homing *)
END_IF;
ELSE
;
END_CASE;
END_IF;
END_IF;
(* 実行條件確認 / execution condition check *)
IF bEnablePls THEN
CASE wAxisStatus OF
2: (* Stopping *)
ErrorID := H1204;
ELSE
;
END_CASE;
END_IF;
(*------------------------------------------------------------------*
* サーボON実行
* Servo ON execution
*------------------------------------------------------------------*)
IF (ErrorID =0) AND bEnablePls AND (wAxisStatus = 0) THEN
G_stLinkBasic.unRWw[wRWw14] := INT_TO_WORD(wPosTrqLimit); (* トルク制限値(正) / Torque limit value(positive) *)
G_stLinkBasic.unRWw[wRWw15] := INT_TO_WORD(wNegTrqLimit); (* トルク制限値(逆) / Torque limit value(negative) *)
G_stLinkBasic.unRWw[wRWw01] := (G_stLinkBasic.unRWw[wRWw01] AND HFF70) OR H000F;
END_IF;
IF ((G_stLinkBasic.unRWr[wRWr02] AND H006F) = H0027) THEN (* OperationEnabled *)
Status := TRUE;
Error := FALSE;
ErrorID := 0;
ELSE
Status := FALSE;
END_IF;
ELSE
wAxisStatus := 1;
ErrorID := H1202;
END_IF;
END_IF;
(*========================================================================*
* エラー出力
* Output for [Error]
*========================================================================*)
IF (ErrorID <> 0) THEN
Error := TRUE;
Status := FALSE;
bBusy := FALSE;
END_IF;
(*========================================================================*
* 有効OFF
* Enable OFF
*========================================================================*)
ELSE
IF bEnablePlf THEN
G_stLinkBasic.unRWw[wRWw01] := (G_stLinkBasic.unRWw[wRWw01] AND HFF70); (* DisableOperation *)
END_IF;
Status := FALSE;
Error := FALSE;
END_IF;
滬公網安備31010802001143號
