(*****************************************************************************
<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;