案例/模板说明
机械臂主控主函数说明
以下为机械臂主控模板代码，包含坐标移动基础模板, 坐标移动二次定位模板和轨迹移动同步模板，轨迹移动异步模板。
坐标移动基础模板
!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] ;
;
坐标移动重定位模板
!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] ;
;
轨迹移动同步模板
;
!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] ;
;
轨迹移动异步模板
!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] ;
;