妗堜緥/妯℃澘璇存槑
============================

鏈烘鑷備富鎺т富鍑芥暟璇存槑
++++++++++++++++++++++++++++++++

鐢ㄦ埛闇€瑕佹牴鎹」鐩儏鍐碉紝缂栧啓鏈烘鑷傚墠鍙颁富绋嬪簭銆�

nachi鐨勪竴閮ㄥ垎瀵勫瓨鍣ㄥ凡缁忚鍗犵敤锛岀敤鎴蜂笉鍏佽浣跨敤杩欎簺瀵勫瓨鍣紝璇﹁nachi api鐣岄潰銆�

浠ヤ笅涓烘満姊拌噦涓绘帶妯℃澘浠ｇ爜(``MZ04-01-A.001``)锛屾敞鎰忓宸ユ帶鏈鸿繑鍥炵殑 err_code 杩涜鍒ゆ柇銆�

.. code-block:: 

    INCLUDE "VARIABLE"

    ' 鍒濆鍖栧苟鍚姩鍚庡彴绋嬪簭
    CallProc MasterInit()
    KILLMCR 300, 0, 10000
    DELAY 2
    FORKMCR 300, 10000
    DELAY 2

    '
    ' 杩炴帴鏈嶅姟绔�
    '
    ' 璁剧疆鏈嶅姟绔痠p鍦板潃(ip鍦板潃鐨勬渶鍚庝竴娈�)
    V3% = 101
    ' 璁剧疆鏈嶅姟绔鍙�
    V4% = 11111
    CallProc xyzMasterConnect()

    '
    ' 鍒囨崲app
    '
    ' 璁剧疆椤圭洰鍚嶇О
    L1$ = "111"
    CallProc xyzSwitchApp(L1$)
    ' 闇€瑕佸杩斿洖鐨別rror_code(V21%)杩涜鍒ゆ柇锛屼互涓嬫瘡鏉¤鍙ュ悓鐞�
    IF V21% <> 0
        STOP
    ENDIF

    *MAIN_LOOP

    '
    ' 璇锋眰pick
    '
    CallProc xyzReqPick()
    IF V21% <> 0
        STOP
    ENDIF

    '
    ' 鑾峰彇pick_in杞ㄨ抗
    '
    CallProc xyzGetPickIn()
    IF V21% <> 0
        STOP
    ENDIF

    ' 杞ㄨ抗鐐规暟閲�>0鏃惰繍琛岃建杩�
    IF V27% > 0
        CallProc ExecTraj()
    ENDIF


    '
    ' 鑾峰彇pick_out杞ㄨ抗
    '
    CallProc xyzGetPickOut()
    IF V21% <> 0
        STOP
    ENDIF

    ' 杞ㄨ抗鐐规暟閲�>0鏃惰繍琛岃建杩�
    IF V27% > 0
        CallProc ExecTraj()
    ENDIF

    '
    ' 鑾峰彇place_in杞ㄨ抗
    '
    CallProc xyzGetPlaceIn()
    IF V21% <> 0
        STOP
    ENDIF

    ' 杞ㄨ抗鐐规暟閲�>0鏃惰繍琛岃建杩�
    IF V27% > 0
        CallProc ExecTraj()
    ENDIF


    '
    ' 鑾峰彇pick_out杞ㄨ抗
    '
    CallProc xyzGetPlaceOut()
    IF V21% <> 0
        STOP
    ENDIF

    ' 杞ㄨ抗鐐规暟閲�>0鏃惰繍琛岃建杩�
    IF V27% > 0
        CallProc ExecTraj()
    ENDIF

    GOTO *MAIN_LOOP



    END



    ' **************************************************************************************************************
    ' SUB FUNCTIONS
    ' **************************************************************************************************************

    UsrProc ExecTraj()
        '
        ' set your own speed/override... here
        ' 濡傛灉闇€瑕佸垎鍒皟鏁村嚭绛�/鍏ョ瓙绛夌殑杞ㄨ抗銆佸渾婊戝€硷紝鍙互鍦ㄨ繖閲岃嚜瀹氫箟璋冩暣
        '
        ' IF V12% = 518
        '     ' set pick_in speed/override...
        ' ELSEIF V12% = 519
        '     ' set pick_out speed/override...
        ' ELSEIF V12% = 520
        '     ' set place_in speed/override...
        ' ELSEIF V12% = 521
        '     ' set place_out speed/override...
        ' ELSEIF V12% = 525
        '     ' set update_obj_pose_to_hand speed/override...
        ' ELSE
        '     STOP
        ' ENDIF


        ' V27%: traj pose num 
        L100% = V27% - 1
        FOR L101% = 0 TO L100% STEP 1
            V19! = V![301+6*L101%]
            V20! = V![302+6*L101%]
            V21! = V![303+6*L101%]
            V22! = V![304+6*L101%]
            V23! = V![305+6*L101%]
            V24! = V![306+6*L101%]
            IF V%[29+L101%] = 11 
                ' JOINTS_MOVEJ
                MOVEX A=3, AC=0, SM=0, M1J, P, (V19!,V20!,V21!,V22!,V23!,V24!),R=100.0,MS
            ELSEIF V%[29+L101%] = 12 
                ' JOINTS_MOVEL
                MOVEX A=3, AC=0, SM=0, M1J, L, (V19!,V20!,V21!,V22!,V23!,V24!),R=100.0,MS
            ELSEIF V%[29+L101%] = 21 
                ' CART_MOVEJ
                MOVEX A=3, AC=0, SM=0, M1X, P, (V19!,V20!,V21!,V22!,V23!,V24!),R=100.0,MS
            ELSEIF V%[29+L101%] = 22 
                ' CART_MOVEL
                MOVEX A=3, AC=0, SM=0, M1X, L, (V19!,V20!,V21!,V22!,V23!,V24!),R=100.0,MS
            ELSE
                STOP
            ENDIF
        NEXT
    EndProc