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

鏈烘鑷備富鎺х▼搴忚鏄�
++++++++++++++++++++++++++++++++
浠ヤ笅涓烘満姊拌噦涓绘帶鐨勫洓绉嶅簲鐢ㄥ舰寮忕殑妯℃澘浠ｇ爜锛屾敞鎰忓宸ユ帶鏈鸿繑鍥炵殑 err_code 杩涜鍒ゆ柇銆�


搴ф爣绉诲姩鍩虹妯℃澘
-------------------------------------

.. code-block:: 

    -------------------------------CartMove Basic Template-------------------------------------

    V_HOME_POSE = {user=0,tool=1,coordinate={240, 560, 90, -153, -14, 16}, joint={80, -50, -100, 90, 90, -30}}
    V_PLACE_POSE = {user=0,tool=1,coordinate={-230, 540, -120, 175, 30, 0}}
    V_SCAN_POSE = {user=0,tool=1,coordinate={-100, 650, 240, -180, 0, 0}}

    function xyzMain()
    -- S1: 鍒濆鍖�
    local err_code = 0
    local return_token = 0
    local ws_id = 0
    local flow_name = "cart_basic.t"
    local item_codename = "item1"

    --IO operation
    DO(1, OFF)

    -- 鍒囨崲浠诲姟娴佸浘 锛岄粯璁よ娉ㄩ噴鎺夛紝濡傛灉闇€姹傚彲浠ュ彇娑堟敞閲�
    -- do 
    --   err_code = xyzSwitchFlow(flow_name)
    --   xyzCheckErrorCode(err_code)
    -- end

    -- S3: 鍒囨崲鎴愬綋鍓嶅伐浠�
    do 
        err_code = xyzSwitchItem(ws_id, item_codename)
        xyzCheckErrorCode(err_code)
    end
    
    -- S4: 杩愬姩鍒� home 浣�
    -- 棣栧厛绉诲姩鍒� home 鐐广€傛敞鎰忛渶瑕佸厛瀹氫箟 V_HOME_POSE 鐨勫叧鑺傝搴﹀€�
    -- 鎴栬€呯敤浣跨敤鍏朵粬鐐规潵鏇夸唬 V_HOME_POSE
    do 
        --Speed(V_J_SPEED)
        --Accel(V_J_ACC)
        --MoveJ(V_HOME_POSE)
        --Sync()

        SpeedS(V_TCP_SPEED)
        AccelS(V_TCP_ACC)
        Move(V_HOME_POSE)
        Sync()
    end

    -- 濡傛灉鏄溂鍦ㄦ満姊拌噦涓婄殑搴旂敤褰㈠紡锛岃鎶� eye_on_hand 璧嬪€间负 true
    eye_on_hand = false

    while(true)
    do
        -- S5: 鐪煎湪鏈烘鑷備笂
        if eye_on_hand then
        -- S6: 杩愬姩鍒版媿鐓т綅濮�
        -- 娉ㄦ剰: 闇€瑕佸厛瀹氫箟鎷嶇収浣嶅Э锛�
        -- 鍙互鐩存帴淇敼 V_SCAN_POSE 鎴栬€呬娇鐢ㄥ叾浠栫偣浣嶆潵鏇挎崲
        --璧板埌鐪煎湪鎵嬩笂鐨勬媿鐓х偣锛屽苟鍙戦€佸綋鍓嶆媿鐓х偣浣嶅Э缁欎笂浣嶆満
        SpeedS(V_TCP_SPEED)
        AccelS(V_TCP_ACC)
        Move(V_SCAN_POSE)
        Sync()

        -- S7: 鍙戦€佹媿鐓т綅濮�
        err_code = xyzSendCurrentCartPose()
        xyzCheckErrorCode(err_code)
        end

        -- S8: 璇锋眰鎶撳彇鐐逛綅
        --璇锋眰璁＄畻鎶撳彇鐐癸紙鑷甫鎷嶇収鍔熻兘锛�
        err_code, return_token = xyzReqGraspPose(ws_id)
        xyzCheckErrorCode(err_code)

        local grasp_pose = {}
        local grasp_pose_num = 0
        local pipeline_num = 0
        local register_num = 0

        -- S9: 鑾峰彇鎶撳彇鐐逛綅
        err_code, grasp_pose, grasp_pose_num, pipeline_num, register_num = xyzGetGraspPose(return_token)
        xyzCheckErrorCode(err_code)

        if grasp_pose_num < 1 then
            --褰撳墠鏃犲彲鎶撳彇宸ヤ欢锛岃繘琛屼笅涓€娆℃媿鐓ц姹�
            print("No grasp_pose, continue requesting")
            Sleep(5000) --5S
            goto Continue
        end

        -- S10: 杩愬姩鍒版姄鍙栫偣骞舵姄鍙栧伐浠�
        -- 娉ㄦ剰: 鍙兘闇€瑕佹坊鍔犲叾浠栬矾寰勭偣浣滀负杩囨浮
        do
        Pick = {user=0,tool=1,coordinate=grasp_pose}
        SpeedS(V_TCP_SPEED)
        AccelS(V_TCP_ACC)
        -- 绉诲姩鍒伴鎶撳彇鐐癸紝鍋忕Щ鍊煎彲浠ユ牴鎹疄闄呴渶姹備慨鏀�
        Move(RP(Pick, {0, 0, 100}))
        -- 杩愬姩鍒版姄鍙栫偣
        Move(Pick)
        Sync()
        DO(1, ON)
        -- 绉诲洖棰勬姄鍙栫偣锛屽亸绉诲€煎彲浠ユ牴鎹疄闄呴渶姹備慨鏀�
        Move(RP(Pick, {0, 0, 100}))
        end

        -- S11: 杩愬姩鍒版斁缃偣骞舵斁缃伐浠�
        -- 娉ㄦ剰: 鍙兘闇€瑕佹坊鍔犲叾浠栬矾寰勭偣浣滀负杩囨浮
        -- 娉ㄦ剰: 棣栧厛闇€瑕佸畾涔夋斁缃偣浣嶅Э,
        -- 鍙互鐩存帴淇敼 V_PLACE_POSE 鎴栬€呬娇鐢ㄥ叾浠栫偣鏉ユ浛鎹�
        do
        SpeedS(V_TCP_SPEED)
        AccelS(V_TCP_ACC)
        Move(V_PLACE_POSE)
        Sync()
        DO(1, OFF)
        end
    end
    end


    -- S2: 杩炴帴鍒板伐鎺ф満
    xyzMasterConnect()
    xyzMain()
    xyzMasterClose()


搴ф爣绉诲姩浜屾瀹氫綅妯℃澘锛�
-------------------------------------

.. code-block::

    -------------------------------CartMove Reposition Template-------------------------------------

    V_HOME_POSE = {user=0,tool=1,coordinate={240, 560, 90, -153, -14, 16}, joint={80, -50, -100, 90, 90, -30}}
    V_PLACE_POSE = {user=0,tool=1,coordinate={-230, 540, -120, 175, 30, 0}}
    V_BOARD_PLACE_POSE = {user=0,tool=1,coordinate={-100, 650, 240, -180, 0, 0}}

    function xyzMain()
    -- S1: 鍒濆鍖�
    local err_code = 0
    local return_token = 0

    local flow_name = "cart_repo.t"
    local item_codename = "item1"

    local ws_id = 0   --鐩告満1
    local ws_id_2 = 1 --鐩告満2
    
    --IO operation
    DO(1,OFF)

    -- 鍒囨崲浠诲姟娴佸浘 锛岄粯璁よ娉ㄩ噴鎺夛紝濡傛灉闇€姹傚彲浠ュ彇娑堟敞閲�
    -- do 
    --   err_code = xyzSwitchFlow(flow_name)
    --   xyzCheckErrorCode(err_code)
    -- end

    -- S3: 鏈烘鑷傚鐨勭浉鏈哄垏鎹㈡垚璇嗗埆褰撳墠宸ヤ欢
    do 
        err_code = xyzSwitchItem(ws_id, item_codename)
        xyzCheckErrorCode(err_code)
    end

    --IO operation
    DO(1,OFF)

    -- S4: 杩愬姩鍒� home 浣�
    -- 棣栧厛绉诲姩鍒� home 鐐广€傛敞鎰忛渶瑕佸厛瀹氫箟 V_HOME_POSE 鐨勫叧鑺傝搴﹀€�
    -- 鎴栬€呯敤浣跨敤鍏朵粬鐐规潵鏇夸唬 V_HOME_POSE
    do 
        --Speed(V_J_SPEED)
        --Accel(V_J_ACC)
        --MoveJ(V_HOME_POSE)
        --Sync()

        SpeedS(V_TCP_SPEED)
        AccelS(V_TCP_ACC)
        Move(V_HOME_POSE)
        Sync()
    end

    while(true)
    do
        local rough_grasp_pose = {}
        local rough_grasp_pose_num = 0
        local rough_pipeline_num = 0
        local rough_register_num = 0
        
        -- S5: 璇锋眰宸ヤ欢鐨勬姄鍙栦綅濮�
        err_code, return_token = xyzReqGraspPose(ws_id)
        xyzCheckErrorCode(err_code)

        -- S6: 鑾峰彇宸ヤ欢鐨勬姄鍙栦綅濮�
        --鐩告満1鑾峰彇宸ヤ欢绮楀畾浣嶆姄鍙栫偣
        err_code, rough_grasp_pose, rough_grasp_pose_num, rough_pipeline_num, rough_register_num = xyzGetGraspPose(return_token)
        xyzCheckErrorCode(err_code)

        -- S7: 澶勭悊鏄惁璇嗗埆鍒板伐浠�
        --鏃犲伐浠讹紝鎶撻殧鏉�
        if rough_grasp_pose_num < 1 then
            local board_grasp_pose = {}
            local board_grasp_pose_num = 0
            local board_pipeline_num = 0
            local board_register_num = 0

            -- S15: 娌℃湁璇嗗埆鍒板伐浠讹紝鏈烘鑷傚鐨勭浉鏈哄垏鎹㈣瘑鍒殧鏉�
            --鐩告満1鎷嶉殧鏉�
            err_code = xyzSwitchItem(ws_id, "board")
            xyzCheckErrorCode(err_code)
            
            -- S16: 璇锋眰闅旀澘鎶撳彇浣嶅Э
            err_code, return_token = xyzReqGraspPose(ws_id)
            xyzCheckErrorCode(err_code)

            -- S17: 鑾峰彇闅旀澘鐨勬姄鍙栦綅濮�
            --鐩告満1鑾峰彇闅旀澘鎶撳彇鐐�
            err_code, board_grasp_pose, board_grasp_pose_num, board_pipeline_num, board_register_num = xyzGetGraspPose(return_token)
            xyzCheckErrorCode(err_code)
            if board_grasp_pose_num < 1 then
            -- no board
            --鏃犻殧鏉匡紝姝ゆ椂涔熷彲浠ユ崲鏂欐
            goto err_exit
            end

            -- S18: 杩愬姩鍒伴殧鏉挎姄鍙栦綅濮匡紝骞舵姄鍙栭殧鏉�
            -- 娉ㄦ剰: 鍙兘闇€瑕佹坊鍔犲叾浠栬矾寰勭偣浣滀负杩囨浮
            --鎶撻殧鏉�
            local P = {user=0,tool=1,coordinate=board_grasp_pose}
            SpeedS(V_TCP_SPEED)
            AccelS(V_TCP_ACC)
            -- 绉诲姩鍒伴殧鏉块鎶撳彇鐐癸紝鍋忕Щ鍊煎彲浠ユ牴鎹疄闄呴渶姹備慨鏀�
            Move(RP(P, {0, 0, 100}))
            -- move to board grasp pose
            Move(P)
            Sync()
            DO(1,ON)
            -- 绉诲洖闅旀澘棰勬姄鍙栫偣锛屽亸绉诲€煎彲浠ユ牴鎹疄闄呴渶姹備慨鏀�
            Move(RP(P, {0, 0, 100}))

            -- S19: 杩愬姩鍒伴殧鏉跨殑鏀剧疆浣嶅Э
            -- 娉ㄦ剰: 鍙兘闇€瑕佹坊鍔犲叾浠栬矾寰勭偣浣滀负杩囨浮
            --鏀剧疆闅旀澘
            SpeedS(V_TCP_SPEED)
            AccelS(V_TCP_ACC)
            Move(V_BOARD_PLACE_POSE)
            Sync()
            DO(1,OFF)
            
            -- S20: 鏈烘鑷傚鐨勭浉鏈哄垏鎹㈡垚璇嗗埆褰撳墠宸ヤ欢
            err_code = xyzSwitchItem(ws_id, item_codename)
            xyzCheckErrorCode(err_code)

        else --鏈夊伐浠讹紝浜屾瀹氫綅
            -- S8: 杩愬姩鍒版媿鐓т綅濮�
            -- 娉ㄦ剰: 闇€瑕佸疄鐜板畾涔夋媿鐓т綅锛屾垨鑰呮牴鎹満姊拌噦澶栫浉鏈烘彁渚涚殑鎶撳彇鐐逛綅鏉ョ‘瀹�, 
            -- 鍙互鐩存帴淇敼 rough_grasp_pose 鎴栬€呬娇鐢ㄥ叾浠栫偣浣嶆浛鎹�
            --璧板埌宸ヤ欢绮楀畾浣嶇偣
            local P = {user=0,tool=1,coordinate=rough_grasp_pose}
            SpeedS(V_TCP_SPEED)
            AccelS(V_TCP_ACC)
            Move(P)
            Sync()

            -- S9: 鍙戦€佹媿鐓т綅濮�
            --鍙戦€佸綋鍓嶇溂鍦ㄦ墜涓婄殑鎷嶇収鐐逛綅濮跨粰涓婁綅鏈�
            err_code = xyzSendCurrentCartPose()
            xyzCheckErrorCode(err_code)

            -- S10: 鏈烘鑷備笂鐨勭浉鏈哄垏鎹㈡垚璇嗗埆褰撳墠宸ヤ欢
            --鐩告満2瀵瑰伐浠惰繘琛岀簿瀹氫綅鎷嶇収
            err_code = xyzSwitchItem(ws_id_2, item_codename)
            xyzCheckErrorCode(err_code)

            -- S11: 璇锋眰鏈烘鑷備笂鐨勭浉鏈鸿幏鍙栧伐浠舵姄鍙栦綅濮�
            err_code, return_token = xyzReqGraspPose(ws_id_2)
            xyzCheckErrorCode(err_code)

            local fine_grasp_pose = {}
            local fine_grasp_pose_num = 0
            local fine_pipeline_num = 0
            local fine_register_num = 0

            -- S12: 鑾峰彇宸ヤ欢鐨勬姄鍙栦綅濮�
            --鑾峰彇宸ヤ欢绮惧畾浣嶆姄鍙栫偣
            err_code, fine_grasp_pose, fine_grasp_pose_num, fine_pipeline_num, fine_register_num = xyzGetGraspPose(return_token)
            xyzCheckErrorCode(err_code)

            if fine_grasp_pose_num < 1 then
            -- no object
            goto err_exit
            end

            -- S13: 杩愬姩鍒板伐浠剁殑鎶撳彇浣嶅Э骞惰繘琛屾姄鍙�
            -- 娉ㄦ剰: 鍙兘闇€瑕佹坊鍔犲叾浠栬矾寰勭偣浣滀负杩囨浮
            --鎶撳伐浠�
            local P = {user=0,tool=1,coordinate=fine_grasp_pose}
            SpeedS(V_TCP_SPEED)
            AccelS(V_TCP_ACC)
            -- 绉诲姩鍒板伐浠堕鎶撳彇鐐癸紝鍋忕Щ鍊煎彲浠ユ牴鎹疄闄呴渶姹備慨鏀�
            Move(RP(P, {0, 0, 100}))
            -- move to grasp pose
            Move(P)
            Sync()
            DO(2, ON)
            -- 绉诲洖宸ヤ欢棰勬姄鍙栫偣锛屽亸绉诲€煎彲浠ユ牴鎹疄闄呴渶姹備慨鏀�
            Move(RP(P, {0, 0, 100}))

            -- S14: 杩愬姩鍒板伐浠剁殑鏀剧疆浣嶅Э
            -- 娉ㄦ剰: 鍙兘闇€瑕佹坊鍔犲叾浠栬矾寰勭偣浣滀负杩囨浮
            --鏀剧疆宸ヤ欢
            SpeedS(V_TCP_SPEED)
            AccelS(V_TCP_ACC)
            Move(V_PLACE_POSE)
            Sync()

            DO(2, OFF)
        end
    end

    ::err_exit:: do
        print("Error_Exit. Please Check")
        return
    end
    end

    -- S2: 杩炴帴鍒板伐鎺ф満
    xyzMasterConnect()
    xyzMain()
    xyzMasterClose()


杞ㄨ抗绉诲姩鍚屾妯℃澘锛�
-------------------------------------


.. code-block::

    -------------------------------TrajMove Sync Template-------------------------------------

    -- 娉ㄦ剰: 浣跨敤 Sync() 鍑芥暟鍙互浣跨▼搴忕瓑寰呮満姊拌噦瀹屾垚杩愬姩鎸囦护
    -- 娉ㄦ剰: 浣跨敤 Sync() 鍑芥暟鍙互浣跨▼搴忕瓑寰呮満姊拌噦瀹屾垚杩愬姩鎸囦护
    -- 娉ㄦ剰: 浣跨敤 Sync() 鍑芥暟鍙互浣跨▼搴忕瓑寰呮満姊拌噦瀹屾垚杩愬姩鎸囦护

    function xyzMain()
        -- S1: 鍒濆鍖�
        local err_code = 0
        local ws_id = 0
        local flow_name = "traj_sync.t"
        local item_codename = "item1"
        
        --IO operation
        DO(1,OFF)
    
        -- 鍒囨崲浠诲姟娴佸浘 锛岄粯璁よ娉ㄩ噴鎺夛紝濡傛灉闇€姹傚彲浠ュ彇娑堟敞閲�
        -- do 
        --     err_code = xyzSwitchFlow(flow_name)
        --     xyzCheckErrorCode(err_code)
        -- end
    
        -- S3: 鍒囨崲鎴愬綋鍓嶅伐浠�
        do 
            err_code = xyzSwitchItem(ws_id, item_codename)
            xyzCheckErrorCode(err_code)
        end
    
        -- S4: 杩愬姩鍒� home 浣�
        -- 棣栧厛绉诲姩鍒� home 鐐广€傛敞鎰忛渶瑕佸厛瀹氫箟 home 鐨勫叧鑺傝搴﹀€�
        -- 鎴栬€呯敤浣跨敤鍏朵粬鐐规潵鏇夸唬 home
        do 
        --local home = {joint={80, -50, -100, 90, 90, -30}}
        --Speed(V_J_SPEED)
        --Accel(V_J_ACC)
        --MoveJ(home)
        --Sync()
    
        local home = {coordinate={240, 560, 90, -153, -14, 16}}
        SpeedS(V_TCP_SPEED)
        AccelS(V_TCP_ACC)
        Move(home)
        Sync()
        end
    
        while(true)
        do
            local pipeline_num = 0
            local register_num = 0
            local wp_num = 0
            local wp_type = {}
            local wp = {}
    
            -- S5: 璇锋眰鎶撳彇鍜屾斁缃鍒�
            err_code = xyzReqPickPlace(ws_id)
            xyzCheckErrorCode(err_code)
    
            -- S6: 鑾峰彇 pick-in 杞ㄨ抗
            err_code, pipeline_num, register_num, wp_num, wp_type, wp = xyzGetPickin(ws_id)
            xyzCheckErrorCode(err_code)
    
            -- S7: 鍒ゆ柇鏄惁璇嗗埆鍒板伐浠�
            --if tote cleared, exit
            if wp_num == 0 then
                print("tote clear")
                goto clear_tore_exit
            end
    
            -- S8: 鎵ц pick-in 杞ㄨ抗
            if xyzExecuteTraj(wp_num, wp_type, wp) ~= 0 then
                print("Execute pick in error")
                goto err_exit
            end
            Sync()
            --IO operation
            DO(1, ON)
    
            -- S9: 鑾峰彇 pick-out 杞ㄨ抗
            err_code, pipeline_num, register_num, wp_num, wp_type, wp = xyzGetPickout(ws_id)
            xyzCheckErrorCode(err_code)
    
            -- S10: 鎵ц pick-out 杞ㄨ抗
            if xyzExecuteTraj(wp_num, wp_type, wp) ~= 0 then
                print("Execute pick out error")
                goto err_exit
            end
    
            -- S11: 鑾峰彇 place-in 杞ㄨ抗
            err_code, pipeline_num, register_num, wp_num, wp_type, wp = xyzGetPlacein(ws_id)
            xyzCheckErrorCode(err_code)
    
            -- S12: 鎵ц place-in 杞ㄨ抗
            if xyzExecuteTraj(wp_num, wp_type, wp) ~= 0 then
                print("Execute place in error")
                goto err_exit
            end
            Sync()
            --IO operation
            DO(1, OFF)
    
            -- S13: 鑾峰彇 place-out 杞ㄨ抗
            err_code, pipeline_num, register_num, wp_num, wp_type, wp = xyzGetPlaceout(ws_id)
            xyzCheckErrorCode(err_code)
    
            -- S14: 鎵ц place-out 杞ㄨ抗
            if xyzExecuteTraj(wp_num, wp_type, wp) ~= 0 then
                print("Execute place out error")
                goto err_exit
            end
        end
    
        -- 澶勭悊鍏朵粬寮傚父鎯呭喌
        ::err_exit:: do
            print("Error_Exit. Please Check the error code")
            return
        end
    
        ::clear_tore_exit:: do
            print("work done: tote has been cleared")
            return
        end
    
    end
    
    -- S2: 杩炴帴鍒板伐鎺ф満
    xyzMasterConnect()
    xyzMain()
    xyzMasterClose()


杞ㄨ抗绉诲姩寮傛妯℃澘锛�
-------------------------------------

.. code-block::

    -------------------------------TrajMove ASync Template-------------------------------------

    -- 娉ㄦ剰: 浣跨敤 Sync() 鍑芥暟鍙互浣跨▼搴忕瓑寰呮満姊拌噦瀹屾垚杩愬姩鎸囦护
    -- 娉ㄦ剰: 浣跨敤 Sync() 鍑芥暟鍙互浣跨▼搴忕瓑寰呮満姊拌噦瀹屾垚杩愬姩鎸囦护
    -- 娉ㄦ剰: 浣跨敤 Sync() 鍑芥暟鍙互浣跨▼搴忕瓑寰呮満姊拌噦瀹屾垚杩愬姩鎸囦护

    function xyzMain()
        -- S1: 鍒濆鍖�
        local err_code = 0
        local ws_id = 0
        local flow_name = "traj_async.t"
        local item_codename = "item1"
        
        --IO operation
        DO(1,OFF)
    
        -- 鍒囨崲浠诲姟娴佸浘 锛岄粯璁よ娉ㄩ噴鎺夛紝濡傛灉闇€姹傚彲浠ュ彇娑堟敞閲�
        -- do 
        --   err_code = xyzSwitchFlow(flow_name)
        --   xyzCheckErrorCode(err_code)
        -- end
    
        -- S3: 鍒囨崲鎴愬綋鍓嶅伐浠�
        do 
        err_code = xyzSwitchItem(ws_id, item_codename)
        xyzCheckErrorCode(err_code)
        end
    
        
    
        -- S4: 杩愬姩鍒� home 浣�
        -- 棣栧厛绉诲姩鍒� home 鐐广€傛敞鎰忛渶瑕佸厛瀹氫箟 home 鐨勫叧鑺傝搴﹀€�
        -- 鎴栬€呯敤浣跨敤鍏朵粬鐐规潵鏇夸唬 home
        do 
        --local home = {joint={80, -50, -100, 90, 90, -30}}
        --Speed(V_J_SPEED)
        --Accel(V_J_ACC)
        --MoveJ(home)
        --Sync()
    
        local home = {coordinate={240, 560, 90, -153, -14, 16}}
        SpeedS(V_TCP_SPEED)
        AccelS(V_TCP_ACC)
        Move(home)
        Sync()
        end
    
        -- S5: 璇锋眰鎶撳彇鍜屾斁缃鍒�
        err_code = xyzReqPickPlace(ws_id)
        xyzCheckErrorCode(err_code)
    
    while(true)
        do
        local pipeline_num = 0
        local register_num = 0
        local wp_num = 0
        local wp_type = {}
        local wp = {}
    
        -- S6: 鑾峰彇 pick-in 杞ㄨ抗
        err_code, pipeline_num, register_num, wp_num, wp_type, wp = xyzGetPickin(ws_id)
        xyzCheckErrorCode(err_code)
    
        -- S7: 鍒ゆ柇鏄惁璇嗗埆鍒板伐浠�
        --if tote cleared, exit
        if wp_num == 0 then
            print("tote clear")
            goto clear_tore_exit
        end
    
        -- S8: 鎵ц pick-in 杞ㄨ抗
        if xyzExecuteTraj(wp_num, wp_type, wp) ~= 0 then
            print("Execute pick in error")
            goto err_exit
        end
        Sync()
        --IO operation
        DO(1, ON)
    
        -- S9: 鑾峰彇 pick-out 杞ㄨ抗
        err_code, pipeline_num, register_num, wp_num, wp_type, wp = xyzGetPickout(ws_id)
        xyzCheckErrorCode(err_code)
    
        -- S10: 鎵ц pick-out 杞ㄨ抗
        if xyzExecuteTraj(wp_num, wp_type, wp) ~= 0 then
            print("Execute pick out error")
            goto err_exit
        end
        Sync()
    
        -- S11: 璇锋眰涓嬩竴娆＄殑鎶撳彇鍜屾斁缃鍒�
        -- request next pick and place plan in advance
        err_code = xyzReqPickPlace(ws_id)
        xyzCheckErrorCode(err_code)
    
        -- S12: 鑾峰彇鏈 place-in 杞ㄨ抗
        err_code, pipeline_num, register_num, wp_num, wp_type, wp = xyzGetPlacein(ws_id)
        xyzCheckErrorCode(err_code)
    
        -- S13: 鎵ц鏈 place-in 杞ㄨ抗
        if xyzExecuteTraj(wp_num, wp_type, wp) ~= 0 then
            print("Execute place in error")
            goto err_exit
        end
        Sync()
        --IO operation
        DO(1, OFF)
    
        -- S14: 鑾峰彇鏈 place-out 杞ㄨ抗
        err_code, pipeline_num, register_num, wp_num, wp_type, wp = xyzGetPlaceout(ws_id)
        xyzCheckErrorCode(err_code)
    
        -- S15: 鎵ц鏈 place-out 杞ㄨ抗
        if xyzExecuteTraj(wp_num, wp_type, wp) ~= 0 then
            print("Execute place out error")
            goto err_exit
        end
        end
    
        -- 澶勭悊鍏朵粬寮傚父鎯呭喌
        ::err_exit:: do
        print("Error_Exit. Please Check the error code")
        return
        end
    
        ::clear_tore_exit:: do
        print("work done: tote has been cleared")
        return
        end
    end
    
    -- S2: 杩炴帴鍒板伐鎺ф満
    xyzMasterConnect()
    xyzMain()
    xyzMasterClose()