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

机械臂主控主函数说明
++++++++++++++++++++++++++++++++

以下为机械臂主控模板代码，包含坐标移动基础模板, 坐标移动二次定位模板，轨迹移动同步模板以及轨迹移动异步模板。
关于模板中的 API 可以查阅 ROKAE 的 “API 说明” 部分。

**请注意模板函数并不能直接运行，请一定根据项目现场环境和流程需求进行修改。建议使用 RobotStudio 来修改。**

坐标移动基础模板
-------------------------------------

.. code-block:: 

    GLOBAL PROC CartMoveBasic()
        VAR int err_code;           // 错误代码
        VAR int flag                // 0 表示眼在手上，1表示眼在手外
        VAR jointtarget home_pose   // home 点
        VAR robtarget scan_pose     // 眼在手上时，需要发送的拍照位
        VAR robtarget place_pose    // 放置位姿

        xyzClose("MasterSock")   // 断开可能已存在的socket
        xyzConnect("MasterSock") // 连接上位机 server
        
        // err_code = xyzSwitchFlow("test.t")    // 切换 flow ，默认被注释掉，如果需求可以取消注释
        // if (err_code != 0)                    // 异常处理
        //     xyzClose("MasterSock")
        // endif

        // 切换工件，两个参数分别表示视觉服务ID，和工件代号
        // 工件代号为MAX中映射表与通讯协议设置中的参数
        err_code = xyzSwitchObject(0, "obj1") 
        if (err_code != 0)                  // 异常处理
            xyzClose("MasterSock")
        endif

        SetDO DO0_0, false // 重置数字输出

        // 首先移动到 home 点。注意需要先定义 home_pose 的关节角度值
        // 或者用使用其他点来替代
        MoveAbsJ home_pose,v1000,z50,tool0

        flag = 1    // 修改手眼关系标志位（是否需要发送拍照点位姿）
        While (true)
		    if (flag==0)
                // 注意需要先定义 scan_pose 的位姿,或者用使用其他点来替代
                MoveL scan_pose,v1000,z50,tool0 // 移动到 scan pose
                err_code = xyzSendCurrentCartPose()  // 发送拍照位姿
                if (err_code != 0)                   // 异常处理
                    xyzClose("MasterSock")
                endif
            endif
            
            err_code = xyzReqGraspPose(0)   // 请求 grasp pose
            if (err_code != 0)              // 异常处理
                xyzClose("MasterSock")
            endif

            // 获取grasp pose
            err_code = xyzGetGraspPose(Token)
            if (err_code != 0)              // 异常处理
                xyzClose("MasterSock")
            endif
            if (PoseNum < 1)    // 识别失败处理
                Wait 5
			    continue
		    endif

            // 可能需要添加其他路径点作为过渡

            // 移动到抓取位姿
            MoveL GraspPose,v1000,z50,tool0
            SetDO DO0_0, true   // 控制数字输出

            // 需要添加其他路径点作为过渡

            // 移动到放置点
            MoveL place_pose,v1000,z50,tool0
            SetDO DO0_0, false  // 重置数字输出
        Endwhile
    ENDPROC


坐标移动二次定位模板
--------------------------------------

.. code-block:: 

    GLOBAL PROC CartMoveRepo()
        VAR int err_code             //错误代码
        VAR jointtarget home_pose    // home 点
        VAR robtarget scan_pose      // 拍照位姿
        VAR robtarget place_pose     // 放置位姿
        
        xyzClose("MasterSock")   // 断开可能已存在的socket
        xyzConnect("MasterSock") // 连接上位机 server
        
        // err_code = xyzSwitchFlow("test.t")    // 切换 flow ，默认被注释掉，如果需求可以取消注释
        // if (err_code != 0)                    // 异常处理
        //     xyzClose("MasterSock")
        // endif

        // 切换工件，两个参数分别表示视觉服务ID，和工件代号
        // 工件代号为MAX中映射表与通讯协议设置中的参数
        err_code = xyzSwitchObject(0, "obj1") 
        if (err_code != 0)                  // 异常处理
            xyzClose("MasterSock")
        endif

        SetDO DO0_0, false // 重置数字输出
	    SetDO DO0_1, false

        // 首先移动到 home 点。注意需要先定义 home_pose 的关节角度值, 或者用使用其他点来替代
        MoveAbsJ home_pose,v1000,z50,tool0
        err_code = xyzReqGraspPose(0)   // 请求 grasp pose
        if (err_code != 0)              // 异常处理
            xyzClose("MasterSock")
        endif

        While (true)
            // 获取抓取位姿
            err_code = xyzGetGraspPose(Token)
            if (err_code != 0)              // 异常处理
                xyzClose("MasterSock")
            endif
            if (PoseNum < 1)                            // 如果识别数量为0
                // 工作空间内没有工件，需要先移除隔板
                err_code = xyzSwitchObject(0, "board") // 切换隔板
                if (err_code != 0)              // 异常处理
                    xyzClose("MasterSock")
                endif

                err_code = xyzReqGraspPose(0);      // 请求隔板的抓取位姿
                if (err_code != 0)              // 异常处理
                    xyzClose("MasterSock")
                endif

                // 获取隔板抓取位姿
                err_code = xyzGetGraspPose(Token)
                if (err_code != 0) OR (pose_num < 1)  // 异常处理与未识别处理
                    xyzClose("MasterSock")
                endif

                // 可能需要添加其他路径点作为过渡

                // 移动到抓取位姿
                MoveL GraspPose,v1000,z50,tool0
                SetDO DO0_0, true   // 控制数字输出

                // 此处需要添加放置隔板的路径点

                err_code = xyzSwitchObject(0, "obj1")    // 切换工件
                if (err_code != 0)              // 异常处理
                    xyzClose("MasterSock")
                endif

                err_code = xyzReqGraspPose(0)   // 请求抓取位姿
                if (err_code != 0)              // 异常处理
                    xyzClose("MasterSock")
                endif
                continue
            else    // 如果识别数量不为0
                // 注意需要先定义 scan_pose 的位姿，或者用使用其他点来替代
                MoveL scan_pose,v1000,z50,tool0   // 运动到 scan pose
			    err_code = xyzSendCurrentCartPose()    // 发送 scan pose
                if (err_code != 0)              // 异常处理
                    xyzClose("MasterSock")
                endif

                err_code = xyzSwitchObject(1, "obj1") // 切换工作空间与工件
                if (err_code != 0)              // 异常处理
                    xyzClose("MasterSock")
                endif
                
                err_code = xyzReqGraspPose(1)  // 请求抓取位姿
                if (err_code != 0)              // 异常处理
                    xyzClose("MasterSock")
                endif
                
                err_code = xyzGetGraspPose(Token) // 获取抓取位姿
                if (err_code != 0) OR (pose_num < 1)  // 异常处理与未识别处理
                    xyzClose("MasterSock")
                endif

                // 可能需要添加其他路径点作为过渡

                // 移动到抓取位姿
                MoveL GraspPose,v1000,z50,tool0
			    SetDO DO0_1,true   // 控制数字输出

                // 可能需要添加其他路径点作为过渡

                // 移动到放置位姿
                MoveL place_pose,v1000,z50,tool0
                SetDO DO0_1,false   // 重置数字输出

            ENDIF
            err_code = xyzSwitchObject(0, "obj1")  // 切换工作空间与工件
            if (err_code != 0)              // 异常处理
                xyzClose("MasterSock")
            endif

            err_code = xyzReqGraspPose(0)   // 请求抓取位姿
            if (err_code != 0)              // 异常处理
                xyzClose("MasterSock")
            endif
        Endwhile
    ENDPROC

轨迹移动同步模板
-------------------------------------

.. code-block:: 

    GLOBAL PROC TrajMoveSync()
        VAR int err_code           // 错误代码
        VAR jointtarget home_pose  // home 点位

        xyzClose("MasterSock")   // 断开可能已存在的socket
        xyzConnect("MasterSock") // 连接上位机 server
        
        // err_code = xyzSwitchFlow("traj_sync.t")    // 切换 flow ，默认被注释掉，如果需求可以取消注释
        // if (err_code != 0)                    // 异常处理
        //     xyzClose("MasterSock")
        // endif

        // 切换工件，两个参数分别表示视觉服务 ID ，和工件代号
        // 工件代号为 MAX 中映射表与通讯协议设置中的参数
        err_code = xyzSwitchObject(0, "obj1") 
        if (err_code != 0)                  // 异常处理
            xyzClose("MasterSock")
        endif

        Reset output0; //重置数字输出

        // 首先移动到 home 点。注意需要先定义 home_pose 的关节角度值，或者用使用其他点来替代
        MoveAbsJ home_pose,v1000,z50,tool0

        While (true)
            // 请求抓取和放置规划
            err_code = xyzReqPickPlace(0)
            if (err_code != 0)              // 异常处理
                xyzClose("MasterSock")
            endif
            
            // 请求 pick in 轨迹
            err_code = xyzGetPickin(0)
            if (err_code != 0)              // 异常处理
                xyzClose("MasterSock")
            endif
            if (PoseNum < 1)
                print("Tote cleared")
                break
            endif
            
            // 执行 pick in 轨迹
            xyzExecuteTraj()
            SetDO DO0_0, true

            // 请求 pick out 轨迹
            err_code = xyzGetPickout(0)
            if (err_code != 0)              // 异常处理
                xyzClose("MasterSock")
            endif
            
            // 执行 pick out 轨迹
            xyzExecuteTraj()
            
            // 请求 place in 轨迹                
            err_code = xyzGetPlacein(0)
            if (err_code != 0)              // 异常处理
                xyzClose("MasterSock")
            endif

            // 执行 place in 轨迹
            xyzExecuteTraj()
            Reset output0;

            // 请求 place out 轨迹
            err_code = xyzGetPlaceout(0)
            if (err_code != 0)              // 异常处理
                xyzClose("MasterSock")
            endif

            // 执行 place out 轨迹
            xyzExecuteTraj()
        Endwhile
    ENDPROC

轨迹移动异步模板
-------------------------------------

.. code-block:: 

    GLOBAL PROC TrajMoveAsync()
        VAR int err_code           // 错误代码
        VAR jointtarget home_pose  // home 点位

        xyzClose("MasterSock")   // 断开可能已存在的socket
        xyzConnect("MasterSock") // 连接上位机 server
        
        // err_code = xyzSwitchFlow("traj_sync.t")    // 切换 flow ，默认被注释掉，如果需求可以取消注释
        // if (err_code != 0)                    // 异常处理
        //     xyzClose("MasterSock")
        // endif

        // 切换工件，两个参数分别表示视觉服务 ID ，和工件代号
        // 工件代号为 MAX 中映射表与通讯协议设置中的参数
        err_code = xyzSwitchObject(0, "obj1") 
        if (err_code != 0)                  // 异常处理
            xyzClose("MasterSock")
        endif

        Reset output0; //重置数字输出

        // 首先移动到 home 点。注意需要先定义 home_pose 的关节角度值，或者用使用其他点来替代
        MoveAbsJ home_pose,v1000,z50,tool0

        err_code = xyzReqPickPlace(0)  // 请求抓取和放置规划
        if (err_code != 0)                  // 异常处理
            xyzClose("MasterSock")
        endif

        While (true)
            // 请求抓取和放置规划
            err_code = xyzReqPickPlace(0)
            if (err_code != 0)              // 异常处理
                xyzClose("MasterSock")
            endif
            
            // 请求 pick in 轨迹
            err_code = xyzGetPickin(0)
            if (err_code != 0)              // 异常处理
                xyzClose("MasterSock")
            endif
            if (PoseNum < 1)
                print("Tote cleared")
                break
            endif
            
            // 执行 pick in 轨迹
            xyzExecuteTraj()
            SetDO DO0_0, true

            // 请求 pick out 轨迹
            err_code = xyzGetPickout(0)
            if (err_code != 0)              // 异常处理
                xyzClose("MasterSock")
            endif
            
            // 执行 pick out 轨迹
            xyzExecuteTraj()
            
            // 运动到相机视野外，请求下一次轨迹运算
            err_code = xyzReqPickPlace(0)  // 请求抓取和放置规划
            if (err_code != 0)                  // 异常处理
                xyzClose("MasterSock")
            endif

            // 继续请求本次 place in 轨迹                
            err_code = xyzGetPlacein(0)
            if (err_code != 0)              // 异常处理
                xyzClose("MasterSock")
            endif

            // 执行 place in 轨迹
            xyzExecuteTraj()
            Reset output0;

            // 继续请求本次 place out 轨迹
            err_code = xyzGetPlaceout(0)
            if (err_code != 0)              // 异常处理
                xyzClose("MasterSock")
            endif

            // 执行 place out 轨迹
            xyzExecuteTraj()
        Endwhile
    ENDPROC