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

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

以下为机械臂主控模板代码，包含坐标移动基础模板、坐标移动二次定位模板、轨迹移动同步模板、轨迹移动异步模板。

关于模板中的 API 可以查阅 Fanuc 的 “API 说明” 部分。

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


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

.. code-block:: 

    :   ;
    :  !S1: 初始化 ;
    :  CALL XYZ_REG_COM    ;
    :  RO[1]=OFF ;
    :   ;
    :   ;
    :  !S2: 运行后台程序，连接到Max ;
    :  RUN XYZ_BKGD ;
    :  WAIT   2.00(sec) ;
    :   ;
    :  IF (R[5:MAST connect fla]<>1) THEN ;
    :  !如果没有连上，则报错 ;
    :  UALM[5] ;
    :  ENDIF ;
    :   ;
    :  ! 切换任务流图，根据需要取消注释 ; 
    :  ! CALL XYZ_SW_TA('cart_basic.t') ;
    :  ! CALL XYZ_CK_ERR    ;
    :  ;
    :  ! 重置任务 ;
    :  CALL XYZ_RESET_TA    ;
    :  CALL XYZ_CK_ERR    ;
    :   ;
    :  !S3: 切换成当前工件 ;
    :  CALL XYZ_SW_ITEM(0,'item1') ;
    :  CALL XYZ_CK_ERR    ;
    :   ;
    :  !S4: 运动到Home点 ;
    :  !需要提前示教Home点 ;
    :J P[1] 100% CNT100    ;
    :   ;
    :  !如果是眼在手上的案例，则将R[101]设置为1，否则设置为0 ;
    :  R[101]=0    ;
    :   ;
    :  LBL[1] ;
    :   ;
    :  !S5: 眼在手上 ;
    :  IF (R[101]=1) THEN ;
    :  !S6: 运动到拍照位姿点 ;
    :  !需要提前示教好拍照位姿并保存到PR寄存器上，且必须是正交形式 ;
    :L PR[41] 100mm/sec FINE    ;
    :  !S7: 发送拍照位姿 ;
    :  CALL XYZ_S_C_POSE(41) ;
    :  CALL XYZ_CK_ERR    ;
    :  ENDIF ;
    :   ;
    :  !S8: 请求抓取目标点位 ;
    :  CALL XYZ_R_G_POSE(0) ;
    :  CALL XYZ_CK_ERR    ;
    :   ;
    :  !S9: 获取抓取目标点位 ;
    :  R[102]=R[11:MAST parsed int]    ;
    :  CALL XYZ_G_G_POSE(R[102]) ;
    :  CALL XYZ_CK_ERR    ;
    :  IF (R[12:MAST pose num]<1) THEN ;
    :  !工作空间中没有工件 ;
    :  WAIT   5.00(sec) ;
    :  JMP LBL[1] ;
    :  ENDIF ;
    :   ;
    :  !S10: 运动到抓取点位 ;
    :  ! 首先走到抓取点位上方;
    :  PR[42]=PR[5:MAST cart pose] ;
    :  PR[42,3]=PR[42,3] + 100 ;
    :L PR[42] 100mm/sec CNT100 ;
    :  ! 抓取 ;
    :L PR[5:MAST cart pose] 100mm/sec FINE    ;
    :  RO[1]=ON ;
    :   ;
    :  !S11: 运动到放置位姿 ;
    :  ! 首先走到抓取点位上方 ;
    :  PR[42]=PR[5:MAST cart pose] ;
    :  PR[42,3]=PR[42,3] + 200 ;
    :L PR[42] 100mm/sec CNT100 ;
    :  ;
    :  ! 走到放置位姿 ;
    :  ! 放置位姿需要提前示教好 ;
    :L P[2] 100mm/sec FINE    ;
    :  RO[1]=OFF ;
    :   ;
    :  JMP LBL[1] ;
    :   ;


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

.. code-block:: 

    :   ;
    :  !S1: 初始化 ;
    :  CALL XYZ_REG_COM    ;
    :  RO[1]=OFF ;
    :  RO[2]=OFF ;
    :   ;
    :  !S2: 运行后台程序，连接到Max ;
    :  RUN XYZ_BKGD ;
    :  WAIT   2.00(sec) ;
    :   ;
    :  IF (R[5:MAST connect fla]<>1) THEN ;
    :  !如果没有连上，则报错 ;
    :  UALM[5] ;
    :  ENDIF ;
    :   ;
    :  ! 切换任务流图，根据需要取消注释 ; 
    :  ! CALL XYZ_SW_TA('cart_repo.t') ;
    :  ! CALL XYZ_CK_ERR    ;
    :  ;
    :  ! 重置任务 ;
    :  CALL XYZ_RESET_TA    ;
    :  CALL XYZ_CK_ERR    ;
    :   ;
    :  !S3: 切换成当前工件 ;
    :  CALL XYZ_SW_ITEM(0,'item1') ;
    :  CALL XYZ_CK_ERR    ;
    :   ;
    :  !S4: 运动到Home点 ;
    :  !需要提前示教Home点 ;
    :J P[1] 100% CNT100    ;
    :   ;
    :  LBL[1] ;
    :   ;
    :  !S5: 请求抓取目标点位 ;
    :  CALL XYZ_R_G_POSE(0) ;
    :  CALL XYZ_CK_ERR    ;
    :   ;
    :  !S6: 获取抓取目标点位 ;
    :  R[101]=R[11:MAST parsed int]    ;
    :  CALL XYZ_G_G_POSE(R[101]) ;
    :  CALL XYZ_CK_ERR    ;
    :   ;
    :  !S7:  处理是否识别到工件 ;
    :  IF (R[12:MAST pose num]<1) THEN ;
    :   ;
    :  !S15: 没有识别到工件，机械臂外的相机切换识别隔板 ;
    :  CALL XYZ_SW_ITEM(0,'board') ;
    :  CALL XYZ_CK_ERR    ;
    :   ;
    :  !S16: 请求隔板抓取位姿 ;
    :  CALL XYZ_R_G_POSE(0) ;
    :  CALL XYZ_CK_ERR    ;
    :   ;
    :  !S17: 获取隔板的抓取位姿 ;
    :  R[101]=R[11:MAST parsed int]    ;
    :  CALL XYZ_G_G_POSE(R[101]) ;
    :  CALL XYZ_CK_ERR    ;
    :  IF (R[12:MAST pose num]<1) THEN ;
    :  !未识别到隔板的处理 ;
    :  JMP LBL[2] ;
    :  ENDIF ;
    :   ;
    :  !S18: 运动到隔板抓取位姿，并抓取隔板 ;
    :  ! 首先走到隔板上方 ;
    :  PR[41]=PR[5:MAST cart pose] ;
    :  PR[41,3]=PR[41,3] + 100 ;
    :L PR[41] 100mm/sec CNT100 ;
    :  ! 抓隔板 ;
    :L PR[5:MAST cart pose] 100mm/sec FINE    ;
    :  RO[1]=ON ;
    :   ;
    :  !S19: 运动到隔板的放置位姿 ;
    :  ! 首先走到隔板上方 ;
    :  PR[41]=PR[5:MAST cart pose] ;
    :  PR[41,3]=PR[41,3] + 200 ;
    :L PR[41] 100mm/sec CNT100 ;
    :  ! 走到隔壁放置位姿 ;
    :  ! 需要提前示教好放置位姿 ;
    :L P[2] 100mm/sec FINE    ;
    :  RO[1]=OFF ;
    :   ;
    :  !S20: 机械臂外的相机切换成识别当前工件 ;
    :  CALL XYZ_SW_ITEM(0,'item1') ;
    :  CALL XYZ_CK_ERR    ;
    :   ;
    :  ELSE ;
    :   ;
    :  !S8: 运动到拍照位姿 ;
    :  !需要提前示教好拍照位姿并保存到PR寄存器上，且必须是正交形式 ;
    :L PR[50] 100mm/sec FINE    ;
    :   ;
    :  !S9: 发送拍照位姿 ;
    :  CALL XYZ_S_C_POSE(50) ;
    :  CALL XYZ_CK_ERR    ;
    :   ;
    :  !S10: 机械臂上的相机切换成识别当前工件 ;
    :  CALL XYZ_SW_ITEM(1,'item1') ;
    :  CALL XYZ_CK_ERR    ;
    :   ;
    :  !S11: 机械臂上的相机请求工件抓取位姿 ;
    :  CALL XYZ_R_G_POSE(1) ;
    :   ;
    :  !S12: 获取工件的抓取位姿 ;
    :  R[101]=R[11:MAST parsed int]    ;
    :  CALL XYZ_G_G_POSE(R[101]) ;
    :   ;
    :  IF (R[12:MAST pose num]<1) THEN ;
    :  JMP LBL[2] ;
    :  ENDIF ;
    :   ;
    :  !S13: 运动到抓取点位 ;
    :  ! 首先走到抓取点位上方 ;
    :  PR[41]=PR[5:MAST cart pose] ;
    :  PR[41,3]=PR[41,3] + 100 ;
    :L PR[41] 100mm/sec CNT100 ;
    :  ! 抓取 ;
    :L PR[5:MAST cart pose] 100mm/sec FINE    ;
    :  RO[2]=ON ;
    :   ;
    :  !S14: 运动到放置位姿  ;
    :  ! 首先走到抓取点位上方 ;
    :  PR[41]=PR[5:MAST cart pose] ;
    :  PR[41,3]=PR[41,3] + 200 ;
    :L PR[41] 100mm/sec CNT100 ;
    :  ! 走到放置位姿 ;
    :  ! 放置位姿需要提前示教好 ;
    :L P[3] 100mm/sec FINE    ;
    :  RO[2]=OFF ;
    :   ;
    :  ENDIF ;
    :   ;
    :  JMP LBL[1] ;
    :   ;
    :  !Handle exception ;
    :  LBL[2] ;
    :  PAUSE ;
    :   ;


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

.. code-block:: 

    :   ;
    :  !S1: 初始化 ;
    :  CALL XYZ_REG_COM    ;
    :  RO[1]=OFF ;
    :   ;
    :  !S2: 运行后台程序，连接到Max  ;
    :  RUN XYZ_BKGD ;
    :  WAIT   2.00(sec) ;
    :   ;
    :  IF (R[5:MAST connect fla]<>1) THEN ;
    :  !如果没有连上，则报错 ;
    :  UALM[5] ;
    :  ENDIF ;
    :   ;
    :  ! 切换任务流图，根据需要取消注释 ; 
    :  ! CALL XYZ_SW_TA('traj_sync.t') ;
    :  ! CALL XYZ_CK_ERR    ;
    :  ;
    :  ! 重置任务 ;
    :  CALL XYZ_RESET_TA    ;
    :  CALL XYZ_CK_ERR    ;
    :   ;
    :  !S3: 切换成当前工件 ;
    :  CALL XYZ_SW_ITEM(0,'item1') ;
    :  CALL XYZ_CK_ERR    ;
    :   ;
    :  !S4: 运动到Home点 ;
    :  !需要提前示教Home点 ;
    :J P[1] 100% FINE    ;
    :   ;
    :  LBL[1] ;
    :   ;
    :  !S5: 请求抓取和放置规划 ;
    :  CALL XYZ_R_PICPLA(0) ;
    :  CALL XYZ_CK_ERR    ;
    :   ;
    :  !S6: 获取 pick-in 轨迹 ;
    :  CALL XYZ_G_PICIN(0) ;
    :  CALL XYZ_CK_ERR    ;
    :   ;
    :  !S7: ;
    :  IF (R[12:MAST pose num]<1) THEN ;
    :  !没有识别到工件 ;
    :  JMP LBL[2] ;
    :  ENDIF ;
    :   ;
    :  !S8: 执行 pick-in 轨迹 ;
    :  CALL XYZ_EXECTRAJ(2) ;
    :  RO[1]=ON ;
    :   ;
    :  !S9: 获取 pick-out 轨迹 ;
    :  CALL XYZ_G_PICOUT(0) ;
    :  CALL XYZ_CK_ERR    ;
    :   ;
    :  !S10: 执行 pick-out 轨迹 ;
    :  CALL XYZ_EXECTRAJ(3) ;
    :   ;
    :  !S11: 获取 place-in 轨迹 ;
    :  CALL XYZ_G_PLAIN(0) ;
    :  CALL XYZ_CK_ERR    ;
    :   ;
    :  !S12: 执行 place-in 轨迹 ;
    :  CALL XYZ_EXECTRAJ(4) ;
    :  RO[1]=OFF ;
    :   ;
    :  !13: 获取 place-out 轨迹 ;
    :  CALL XYZ_G_PLAOUT(0) ;
    :  CALL XYZ_CK_ERR    ;
    :   ;
    :  !S14: 执行 place-out 轨迹 ;
    :  CALL XYZ_EXECTRAJ(5) ;
    :  JMP LBL[1] ;
    :   ;
    :  !Hanle no object in LBL2 ;
    :  LBL[2] ;
    :  PAUSE ;
    :   ;


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

.. code-block:: 

    :   ;
    :  !S1: 初始化 ;
    :  CALL XYZ_REG_COM    ;
    :  RO[1]=OFF ;
    :   ;
    :  !S2: 运行后台程序，连接到Max ;
    :  RUN XYZ_BKGD ;
    :  WAIT   2.00(sec) ;
    :   ;
    :  IF (R[5:MAST connect fla]<>1) THEN ;
    :  !如果没有连上，则报错 ;
    :  UALM[5] ;
    :  ENDIF ;
    :   ;
    :  ! 切换任务流图，根据需要取消注释 ; 
    :  ! CALL XYZ_SW_TA('cart_basic.t') ;
    :  ! CALL XYZ_CK_ERR    ;
    :  ;
    :  ! 重置任务 ;
    :  CALL XYZ_RESET_TA    ;
    :  CALL XYZ_CK_ERR    ;
    :   ;
    :  !S3: 切换成当前工件 ;
    :  CALL XYZ_SW_ITEM(0,'item1') ;
    :  CALL XYZ_CK_ERR    ;
    :   ;
    :  !S4: 运动到Home点 ;
    :  !需要提前示教Home点 ;
    :J P[1] 100% CNT100    ;
    :   ;
    :  !S5: 请求抓取和放置规划 ;
    :  CALL XYZ_R_PICPLA(0) ;
    :  CALL XYZ_CK_ERR    ;
    :   ;
    :   ;
    :  LBL[1] ;
    :   ;
    :  !S6: 获取 pick-in 轨迹 ;
    :  CALL XYZ_G_PICIN(0) ;
    :  CALL XYZ_CK_ERR    ;
    :   ;
    :  !S7 ;
    :  IF (R[12:MAST pose num]<1) THEN ;
    :  !没有识别到工件 ;
    :  JMP LBL[2] ;
    :  ENDIF ;
    :   ;
    :  !S8: 执行 pick-in 轨迹 ;
    :  CALL XYZ_EXECTRAJ(2) ;
    :  RO[1]=ON ;
    :   ;
    :  !S9: 获取 pick-out 轨迹 ;
    :  CALL XYZ_G_PICOUT(0) ;
    :  CALL XYZ_CK_ERR    ;
    :   ;
    :  !S10: 执行 pick-out 轨迹 ;
    :  CALL XYZ_EXECTRAJ(3) ;
    :   ;
    :  !S11: 请求下一次的抓取和放置规划 ;
    :  CALL XYZ_R_PICPLA(0) ;
    :  CALL XYZ_CK_ERR    ;
    :   ;
    :  !S12: 获取本次 place-in 轨迹 ;
    :  CALL XYZ_G_PLAIN(0) ;
    :  CALL XYZ_CK_ERR    ;
    :   ;
    :  !S13: 执行本次 place-in 轨迹 ;
    :  CALL XYZ_EXECTRAJ(4) ;
    :  RO[1]=OFF ;
    :   ;
    :  !S14: 获取本次 place-out 轨迹 ;
    :  CALL XYZ_G_PLAOUT(0) ;
    :  CALL XYZ_CK_ERR    ;
    :   ;
    :  !S15: 执行本次 place-out 轨迹 ;
    :  CALL XYZ_EXECTRAJ(5) ;
    :  JMP LBL[1] ;
    :   ;
    :  !Handle no object ;
    :  LBL[2] ;
    :  PAUSE ;
    :   ;