案例/模板说明
============================

机械臂主控程序说明
++++++++++++++++++++++++++++++++
以下为机械臂主控的四种应用形式的模板代码，注意对工控机返回的 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()