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

注意事项：

- **请注意模板函数并不能直接运行，请一定根据项目现场环境和流程需求进行修改**
- **需要对Max返回的 error_code 进行判断**

机械臂主控模板说明
++++++++++++++++++++++++++++++++

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

模板中涉及示教部分点位，使用 ``RT ToolBox3`` 示教点位操作如下：

.. figure:: img/pose_teach.png
    :width: 885px
    :align: center

    示教点位

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

CartMove基本模板： XYZCARTBASIC.prg

.. code-block:: 

    '
    '
    Title "XYZ CartMove Basic Template"
    '
    '
    '------------------------------
    ' S1: 初始化
    '------------------------------
    '
    '
    #Include "XYZUTILS"
    Def Act 3, M_Err > 0 GoSub *LSYSERR
    Act 3 = 1
    xyzInitParams()
    '
    '
    '------------------------------
    ' S2: 连接Max，机器人做客户端
    '------------------------------
    '
    '
    xyzConnect("192.168.37.101",11111) ' 服务器ip地址和端口号
    '
    M1 = xyzResetTask()
    xyzCheckErrorCode(M1)
    '
    '
    ' switch task
    '
    ' M1 = xyzSwitchTask("cart_basic.t")
    ' xyzCheckErrorCode(M1)
    '
    '
    '------------------------------
    ' S3: 切换工件
    '------------------------------
    '
    '
    M1 = xyzSwitchItem(0, "item1") 
    xyzCheckErrorCode(M1)
    '
    '
    '------------------------------
    ' S4: 运动到JHome点，JHome点需要提前示教好
    '------------------------------
    '
    '
    M_Out(10) = 0
    Mov JHome
    '
    '
    ' 如果项目是眼在手上，则设置为1，否则设置为0
    MIsEyeInHand = 1
    '
    '
    *LOOP
    '
    '
    '------------------------------
    ' S5: 如果是眼在手上的场景
    '------------------------------
    '
    '
    If MIsEyeInHand = 1 Then
        '
        '
        '------------------------------
        ' S6: 则运动到拍照点，拍照点需要提前示教
        '------------------------------
        '
        '
        Mvs PScanPose
        '
        '
        '------------------------------
        ' S7: 发送拍照位姿
        '------------------------------
        '
        '
        M1 = xyzSendCurrentCartPose()
        xyzCheckErrorCode(M1)
        '
    EndIf
    '
    '
    '------------------------------
    ' S8 + S9: 拍照并获取抓取点
    '
    ' 该指令等价于执行：请求抓取点位 + 获取抓取点位
    '------------------------------
    '
    '
    M1 = xyzCalculateGraspPose(0)
    xyzCheckErrorCode(M1)
    ' check grasp pose num
    If M_01# < 1 Then
        ' 未识别到工件，重新开始
        Dly 5
        GoTo *LOOP
    EndIf
    '
    '
    '------------------------------
    ' S10: 运动到抓取点并抓取工件
    '
    ' Note:
    '   可能需要添加其他路径点作为过渡
    '------------------------------
    '
    '
    ' 预抓取点
    Mvs P_00,-100
    ' 抓取工件
    Mvs P_00
    M_Out(10) = 1
    Dly 0.5
    ' 抬升
    Mvs P_100,-100
    '
    '
    '------------------------------
    ' S11: 运动到放置点并放置
    '
    ' Note:
    '   放置点PPlacePose需要提前示教好
    '------------------------------
    '
    '
    Mvs PPlacePose
    M_Out(10) = 0
    Dly 0.5
    '
    '
    GoTo *LOOP
    '
    '
    '------------------------------
    ' System error
    '------------------------------
    '
    '
    *LSYSERR:
    '
    ' system error
    '
    Hlt
    End

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

CartMove二次定位模板： XYZCARTREPO.prg

.. code-block:: 

    '
    '
    Title "XYZ CartMove Repositioning Template"
    '
    '
    '------------------------------
    ' S1: 初始化
    '------------------------------
    '
    '
    #Include "XYZUTILS"
    Def Act 3, M_Err > 0 GoSub *LSYSERR
    Act 3 = 1
    xyzInitParams()
    '
    '
    '------------------------------
    ' S2: 连接Max，机器人做客户端
    '------------------------------
    '
    '
    xyzConnect("192.168.37.101",11111) ' 服务器ip地址和端口号
    '
    M1 = xyzResetTask()
    xyzCheckErrorCode(M1)
    '
    ' switch task
    '
    '
    ' M1 = xyzSwitchTask("cart_repo.t")
    ' xyzCheckErrorCode(M1)
    '
    '
    '------------------------------
    ' S3: 眼在手外相机切换为识别当前工件
    '------------------------------
    '
    '
    M1 = xyzSwitchItem(0, "item1")
    xyzCheckErrorCode(M1)
    '
    '
    '
    '------------------------------
    ' S4: 运动到JHome点，JHome点需要提前示教好
    '------------------------------
    '
    '
    M_Out(10) = 0 ' 抓取隔板的信号
    M_Out(11) = 0 ' 抓取工件的信号
    Mov JHome
    '
    '
    '
    '
    *LOOP
    '
    '
    '------------------------------
    ' S5 + S6: 拍照并获取工件的抓取点
    '
    ' 该指令等价于执行：请求抓取点位 + 获取抓取点位
    '------------------------------
    '
    '
    M1 = xyzCalculateGraspPose(0)
    xyzCheckErrorCode(M1)
    '
    '
    '------------------------------
    ' S7: 判断是否识别到工件
    '------------------------------
    '
    '
    If M_01# < 1 Then ' 未识别到工件
        '
        '
        '------------------------------
        ' S15: 没有识别到工件，眼在手外相机切换成识别隔板
        '
        '------------------------------
        M1 = xyzSwitchItem(0, "board")
        xyzCheckErrorCode(M1)
        '
        '
        '------------------------------
        ' S16 + S17: 拍照并获取隔板的抓取点
        '------------------------------
        '
        '
        M1 = xyzCalculateGraspPose(0)
        xyzCheckErrorCode(M1)
        If M_01# < 1 Then Hlt ' 未识别到隔板
        '
        '------------------------------
        ' S18: 抓隔板
        '------------------------------
        '
        '
        ' 预抓取点
        Mvs P_00, -100
        ' 抓取隔板
        Mvs P_00
        M_Out(10) = 1
        Dly 0.5
        ' 抬升
        Mvs P_00, -100
        '
        '
        '------------------------------
        ' S19: 放置隔板
        ' Note:
        '   放置位置PBoardPlacePose需要提前示教好
        '------------------------------
        '
        '
        Mvs PBoardPlacePose
        M_Out(10) = 0
        Dly 0.5
        '
        '
        '------------------------------
        ' S20: 眼在手外相机切换为识别当前工件
        '------------------------------
        '
        '
        M1 = xyzSwitchItem(0, "item1")
        xyzCheckErrorCode(M1)
        '
        '
        GoTo *LOOP
        '
        '
    EndIf
    '
    '
    '------------------------------
    ' S8: 运动到拍照位姿，这里是运动到粗定位点
    '------------------------------
    '
    '
    Mvs P_00
    '
    '
    '------------------------------
    ' S9: 发送拍照位姿
    '------------------------------
    '
    '
    M1 = xyzSendCurrentCartPose()
    xyzCheckErrorCode(M1)
    '
    '------------------------------
    ' S10: 眼在手上相机切换为识别当前工件
    '------------------------------
    '
    '
    M1 = xyzSwitchItem(1, "item1")
    xyzCheckErrorCode(M1)
    '
    '
    '------------------------------
    ' S11 + S12: 拍照并获取工件的精定位点
    '------------------------------
    '
    '
    M1 = xyzCalculateGraspPose(1)
    xyzCheckErrorCode(M1)
    If M_01# < 1 Then Hlt ' 未识别到工件
    '
    '
    '------------------------------
    ' S13: 抓取工件
    '------------------------------
    '
    '
    ' 预抓取点
    Mvs P_00,-100
    ' 抓取工件
    Mvs P_00
    M_Out(11) = 1
    Dly 0.5
    ' 抬升
    Mvs P_100,-100
    '
    '
    '------------------------------
    ' S14: 放置工件
    ' Note:
    '   放置位置PItemPlacePose需要提前示教好
    '------------------------------
    '
    '
    Mvs PItemPlacePose
    M_Out(11) = 0
    Dly 0.5
    '
    '
    GoTo *LOOP
    '
    '
    '------------------------------
    ' System error
    '------------------------------
    *LSYSERR:
    '
    ' system error
    '
    Hlt
    End


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

同步模板：XYZTRAJSYNC.prg

.. code-block:: 

    '
    '
    Title "XYZ TrajMove Sync Template"
    '
    '
    '------------------------------
    ' S1: 初始化
    '------------------------------
    '
    '
    #Include "XYZUTILS"
    Def Act 3, M_Err > 0 GoSub *LSYSERR
    Act 3 = 1
    xyzInitParams()
    '
    '
    '------------------------------
    ' S2: 连接Max，机器人做客户端
    '------------------------------
    '
    '
    xyzConnect("192.168.37.101",11111) ' 服务器ip地址和端口号
    '
    M1 = xyzResetTask()
    xyzCheckErrorCode(M1)
    '
    ' switch task
    '
    ' M1 = xyzSwitchTask("traj_sync.t")
    ' xyzCheckErrorCode(M1)
    '
    '
    '------------------------------
    ' S3: 切换工件
    '------------------------------
    '
    '
    M1 = xyzSwitchItem(0, "item1")
    xyzCheckErrorCode(M1)
    '
    '
    '------------------------------
    ' S4: 运动到JHome点，JHome点需要提前示教好
    '------------------------------
    '
    '
    M_Out(10) = 0
    Mov JHome
    '
    '
    '
    '
    *LOOP
    '
    '
    '------------------------------
    ' S5: 请求Pick和Place规划
    '------------------------------
    '
    '
    M1 = xyzReqPickPlace(0)
    xyzCheckErrorCode(M1)
    '
    '
    '------------------------------
    ' S6: 获取 pick-in 轨迹
    '------------------------------
    '
    '
    M1 = xyzGetPickIn(0)
    xyzCheckErrorCode(M1)
    '
    '
    '------------------------------
    ' S7: 判断轨迹点数量，小于1表示未识别到工件
    '------------------------------
    '
    '
    If M_05# < 1 Then
        GoTo *CLEAREXIT
    EndIf
    '
    '
    '------------------------------
    ' S8: 执行 pick-in 轨迹
    '------------------------------
    '
    '
    '   50表示执行轨迹时,最大圆滑半径为50mm
    xyzExecPickInTraj(50)
    M_Out(10) = 1
    Dly 0.5
    '
    '
    '------------------------------
    ' S9: 获取 pick-out 轨迹 
    '------------------------------
    '
    '
    M1 = xyzGetPickOut(0)
    xyzCheckErrorCode(M1)
    '
    '
    '------------------------------
    ' S10: 执行 pick-out 轨迹
    '------------------------------
    '
    '
    xyzExecPickOutTraj(50)
    '
    '
    '------------------------------
    ' S11: 获取 place-in 轨迹
    '------------------------------
    '
    M1 = xyzGetPlaceIn(0)
    xyzCheckErrorCode(M1)
    '
    '
    '------------------------------
    ' S12: 执行 place-in 轨迹
    '------------------------------
    '
    '
    xyzExecTraj(50)
    M_Out(10) = 0
    Dly 0.5
    '
    '
    '------------------------------
    ' S13: 获取 place-out 轨迹
    '------------------------------
    '
    M1 = xyzGetPlaceOut(0)
    xyzCheckErrorCode(M1)
    '
    '
    '------------------------------
    ' S14: 执行 place-out 轨迹
    '------------------------------
    xyzExecTraj(50)
    '
    '
    '
    GoTo *LOOP
    '
    '
    '------------------------------
    ' Normal exit
    '------------------------------
    '
    *CLEAREXIT
    ' 清框，工作结束
    ' work done:tote has beeen cleared
    Hlt
    End
    '
    '
    '------------------------------
    ' System error
    '------------------------------
    *LSYSERR:
    '
    ' system error
    '
    Hlt
    End


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

异步模板：XYZTRAJASYNC.prg

.. code-block:: 

    '
    '
    Title "XYZ TrajMove Async Template"
    '
    '
    '------------------------------
    ' S1: 初始化
    '------------------------------
    '
    '
    #Include "XYZUTILS"
    Def Act 3, M_Err > 0 GoSub *LSYSERR
    Act 3 = 1
    xyzInitParams()
    '
    '
    '------------------------------
    ' S2: 连接Max，机器人做客户端
    '------------------------------
    '
    '
    xyzConnect("192.168.37.101",11111) ' 服务器ip地址和端口号
    '
    M1 = xyzResetTask()
    xyzCheckErrorCode(M1)
    '
    ' switch task
    '
    ' M1 = xyzSwitchTask("traj_async.t")
    ' xyzCheckErrorCode(M1)
    '
    '
    '------------------------------
    ' S3: 切换工件
    '------------------------------
    '
    '
    M1 = xyzSwitchItem(0, "item1")
    xyzCheckErrorCode(M1)
    '
    '
    '
    '------------------------------
    ' S4: 运动到JHome点，JHome点需要提前示教好
    '------------------------------
    '
    '
    M_Out(10) = 0
    Mov JHome
    '
    '
    '------------------------------
    ' S5: 请求Pick和Place规划
    '------------------------------
    '
    '
    M1 = xyzReqPickPlace(0)
    xyzCheckErrorCode(M1)
    '
    '
    '
    *LOOP
    '
    '
    '------------------------------
    ' S6: 获取 pick-in 轨迹
    '------------------------------
    '
    '
    M1 = xyzGetPickIn(0)
    xyzCheckErrorCode(M1)
    '
    '
    '------------------------------
    ' S7: 判断轨迹点数量，小于1表示未识别到工件
    '------------------------------
    '
    '
    If M_05# < 1 Then
        GoTo *CLEAREXIT
    EndIf
    '
    '
    '------------------------------
    ' S8: 执行 pick-in 轨迹
    '------------------------------
    '
    '
    '   50表示执行轨迹时,最大圆滑半径为50mm
    xyzExecPickInTraj(50)
    M_Out(10) = 1
    Dly 0.5
    '
    '
    '------------------------------
    ' S9: 获取 pick-out 轨迹 
    '------------------------------
    '
    '
    M1 = xyzGetPickOut(0)
    xyzCheckErrorCode(M1)
    '
    '
    '------------------------------
    ' S10: 执行 pick-out 轨迹
    '------------------------------
    '
    '
    xyzExecPickOutTraj(50)
    '
    '
    '------------------------------
    ' S11: 提前触发下一次的Pick和Place规划
    '------------------------------
    '
    '
    M1 = xyzReqPickPlace(0)
    xyzCheckErrorCode(M1)
    '
    '
    '------------------------------
    ' S12: 获取本次的 place-in 轨迹
    '------------------------------
    '
    M1 = xyzGetPlaceIn(0)
    xyzCheckErrorCode(M1)
    '
    '
    '------------------------------
    ' S13: 执行本次的 place-in 轨迹
    '------------------------------
    '
    '
    xyzExecTraj(50)
    M_Out(10) = 0
    Dly 0.5
    '
    '
    '------------------------------
    ' S14: 获取本次的 place-out 轨迹
    '------------------------------
    '
    M1 = xyzGetPlaceOut(0)
    xyzCheckErrorCode(M1)
    '
    '
    '------------------------------
    ' S15: 执行本次的 place-out 轨迹
    '------------------------------
    '
    '
    xyzExecTraj(50)
    '
    '
    GoTo *LOOP
    '
    '
    '------------------------------
    ' Normal exit
    '------------------------------
    '
    *CLEAREXIT
    ' 清框，工作结束
    ' work done:tote has beeen cleared
    Hlt
    End
    '
    '
    '------------------------------
    ' System error
    '------------------------------
    *LSYSERR:
    '
    ' system error
    '
    Hlt
    End