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

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

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

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

.. code-block:: 

     !S1: 初始化 ;
     CALL XYZ_REG_COM    ;
     RO[1]=OFF ;
      ;
      ;
     !S2: 连接到工控机;
     RUN XYZ_BKGD ;
     WAIT   1.00(sec) ;
       ;
      IF (R[5]<>1) THEN ;
      !如果没连接到工控机，示教器会报警提示 ;
      UALM[5] ;
      ENDIF ;
       ;
       !切换任务流图 ，默认被注释掉，如果需求可以取消注释
      //CALL XYZ_SW_FLW('cart_basic.t') ;
      //CALL XYZ_CK_ERR    ;
       ;
      !S3: 切换成当前工件 ;
      CALL XYZ_SW_ITEM(0,'item1') ;
      CALL XYZ_CK_ERR    ;
       ;
      ! S4: 运动到 home 位 ;
      ! 首先移动到 home 点。注意需要先定义 home_pose 的关节角度值;
      ! 或者用使用其他点来替代 ;
    J P[1:home pose] 100% CNT100    ;
       ;
      !如果是眼在机械臂上的应用形式 ;
      !请修改 R[99]=1 ;
      R[99]=0    ;
       ;
      LBL[1] ;
       ;
      !S5: 眼在机械臂上 ;
      IF (R[99]=1) THEN ;
      !S6: 运动到拍照位姿 ;
      !注意需要先定义 scan_pose 的位姿 ;
      !或者用使用其他点来替代 ;
      !scan pose 必须定义在位置寄存器里，不然后台读取不到 ;
    L PR[99] 100mm/sec FINE    ;
      !S7: 发送拍照位姿 ;
      CALL XYZ_S_C_POSE(99) ;
      CALL XYZ_CK_ERR    ;
      ENDIF ;
       ;
      !S8: 请求抓取点位 ;
      CALL XYZ_R_G_POSE(0) ;
      CALL XYZ_CK_ERR    ;
       ;
      !S9: 获取抓取点位 ;
      R[98]=R[11:MAST parsed int]    ;
      CALL XYZ_G_G_POSE(R[98]) ;
      CALL XYZ_CK_ERR    ;
      IF (R[12:MAST pose num]<1) THEN ;
      !工作空间中没有工件 ;
      WAIT   5.00(sec) ;
      JMP LBL[1] ;
      ENDIF ;
       ;
      !S10: 运动到抓取点并抓取工件 ;
      !这里需要添加过渡点，防止机器人运动过程中发生碰撞 ;
    L PR[5:MAST cart pose] 100mm/sec FINE    ;
      RO[1]=ON ;
       ;
      !S11: 运动到放置点并放置工件  ;
      !这里需要添加过渡点，防止机器人运动过程中发生碰撞 ;
    L P[2:place pose] 100mm/sec FINE    ;
      RO[1]=OFF ;
       ;
      JMP LBL[1] ;
       ;


坐标移动重定位模板
--------------------------------------

.. code-block:: 

     !S1: 初始化 ;
     CALL XYZ_REG_COM    ;
     RO[1]=OFF ;
     RO[2]=OFF ;
      ;
     !S2: 连接到工控机 ;
     RUN XYZ_BKGD ;
     WAIT   1.00(sec) ;
       ;
      IF (R[5]<>1) THEN ;
      !如果没连接到工控机，示教器会报警提示 ;
      UALM[5] ;
      ENDIF ;
       ;
       !切换任务流图 ，默认被注释掉，如果需求可以取消注释
      //CALL XYZ_SW_FLW('cart_repo.t') ;
      //CALL XYZ_CK_ERR    ;
       ;
      !S3: 机械臂外的相机切换成识别当前工件;
      CALL XYZ_SW_ITEM(0,'item1') ;
      CALL XYZ_CK_ERR    ;
       ;
      !S4: 运动到home pose ;
      ! 首先移动到 home 点。注意需要先定义 home_pose 的关节角度值;
      ! 或者用使用其他点来替代 ;
    J P[1:home pose] 100% CNT100    ;
       ;
      LBL[1] ;
       ;
      !S5: 请求工件的抓取位姿 ;
      CALL XYZ_R_G_POSE(0) ;
      CALL XYZ_CK_ERR    ;
       ;
      !S6: 获取工件的抓取位姿 ;
      R[98]=R[11:MAST parsed int]    ;
      CALL XYZ_G_G_POSE(R[98]) ;
      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[98]=R[11:MAST parsed int]    ;
      CALL XYZ_G_G_POSE(R[98]) ;
      CALL XYZ_CK_ERR    ;
      IF (R[12:MAST pose num]<1) THEN ;
      !no board.maybe tote empty ;
      JMP LBL[2]
      ENDIF ;
       ;
      !S18: 运动到隔板抓取位姿，并抓取隔板 ;
      !这里需要添加过渡点，防止机器人运动过程中发生碰撞 ;
    L PR[5:MAST cart pose] 100mm/sec FINE    ;
      RO[1]=ON ;
       ;
      !S19: 运动到隔板的放置位姿 ;
      !这里需要添加过渡点，防止机器人运动过程中发生碰撞 ;
    L P[3:board place pose] 100mm/sec FINE    ;
      RO[1]=OFF ;
       ;
      !S20: 机械臂外的相机切换成识别当前工件;
      CALL XYZ_SW_ITEM(0,'item1') ;
      CALL XYZ_CK_ERR    ;
       ;
      ELSE ;
       ;
      !S8: 运动到拍照位姿 ;
      !注意需要先定义 scan_pose 的位姿 ;
      !或者用使用其他点来替代 ;
    L PR[99] 100mm/sec FINE    ;
       ;
      !S9: 发送拍照位姿 ;
      CALL XYZ_S_C_POSE(99) ;
      CALL XYZ_CK_ERR    ;
       ;
      !S10: 机械臂上的相机切换成识别当前工件;
      CALL XYZ_SW_ITEM(1,'item1') ;
      CALL XYZ_CK_ERR    ;
       ;
      !S11: 请求机械臂上的相机获取工件抓取位姿 ;
      CALL XYZ_R_G_POSE(1) ;
       ;
      !S12: 获取工件的抓取位姿 ;
      R[98]=R[11:MAST parsed int]    ;
      CALL XYZ_G_G_POSE(R[98]) ;
       ;
      IF (R[12:MAST pose num]<1) THEN ;
      JMP LBL[2] ;
      ENDIF ;
       ;
      !S13: 运动到工件的抓取位姿并进行抓取 ;
      !这里需要添加过渡点，防止机器人运动过程中发生碰撞 ;
    L PR[5:MAST cart pose] 100mm/sec FINE    ;
      RO[2]=ON ;
      ;
      !S14: 运动到工件的放置位姿 ;
      !这里需要添加过渡点，防止机器人运动过程中发生碰撞 ;
    L P[4:object place pos] 100mm/sec FINE    ;
      RO[2]=OFF ;
      ;
      ENDIF ;
      ;
      JMP LBL[1] ;
      ;
      !处理未识别到工件的情况 ;
      LBL[2] ;
      ;


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

.. code-block:: 

    
      ;
     !S1: 初始化 ;
     CALL XYZ_REG_COM    ;
     RO[1]=OFF ;
      ;
     !S2: 连接到工控机 ;
     RUN XYZ_BKGD ;
     WAIT   1.00(sec) ;
      ;
      IF (R[5]<>1) THEN ;
      !如果没连接到工控机，示教器会报警提示 ;
      UALM[5] ;
      ENDIF ;
       ;
       !切换任务流图 ，默认被注释掉，如果需求可以取消注释
      //CALL XYZ_SW_FLW('traj_async.t') ;
      //CALL XYZ_CK_ERR    ;
       ;
      !S3: 切换成当前工件 ;
      CALL XYZ_SW_ITEM(0,'item1') ;
      CALL XYZ_CK_ERR    ;
       ;
      !S4: 运动到home pose ;
      ! 首先移动到 home 点。注意需要先定义 home_pose 的关节角度值;
      ! 或者用使用其他点来替代 ;
    J P[1:home pose] 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]<>0) THEN ;
      !no object ;
      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) ;
       ;
      !13: 获取 place-out 轨迹;
      CALL XYZ_G_PLAOUT(0) ;
      CALL XYZ_CK_ERR    ;
       ;
      !S14: 执行 place-out 轨迹;
      CALL XYZ_EXECTRAJ(5) ;
      JMP LBL[1] ;
       ;
      !处理未识别到工件的情况 ;
      LBL[2] ;
       ;


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

.. code-block:: 

      !S1: 初始化 ;
      CALL XYZ_REG_COM    ;
      RO[1]=OFF ;
       ;
      !S2: 连接到工控机 ;
      RUN XYZ_BKGD ;
      WAIT   1.00(sec) ;
       ;
      IF (R[5]<>1) THEN ;
      !如果没连接到工控机，示教器会报警提示 ;
      UALM[5] ;
      ENDIF ;
       ;
       !切换任务流图 ，默认被注释掉，如果需求可以取消注释
      //CALL XYZ_SW_FLW('traj_sync.t') ;
      //CALL XYZ_CK_ERR    ;
       ;
      !S3: 切换成当前工件 ;
      CALL XYZ_SW_ITEM(0,'item1') ;
      CALL XYZ_CK_ERR    ;
       ;
      !S4: 运动到home pose ;
      ! 首先移动到 home 点。注意需要先定义 home_pose 的关节角度值;
      ! 或者用使用其他点来替代 ;
    J P[1:home pose] 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]=0) THEN ;
      !no object ;
      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) ;
       ;
      !S14: 获取本次 place-out 轨迹;
      CALL XYZ_G_PLAOUT(0) ;
      CALL XYZ_CK_ERR    ;
       ;
      !S15: 执行本次 place-out 轨迹;
      CALL XYZ_EXECTRAJ(5) ;
      JMP LBL[1] ;
       ;
      !处理未识别到工件的情况 ;
      LBL[2] ;
       ;