案例/模板说明

注意事项：

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

  • 需要对Max返回的 error_code 进行判断

机械臂主控模板说明

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

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

../../../_images/pose_teach.png

示教点位

坐标移动基础模板

CartMove基本模板： XYZCARTBASIC.prg

'
'
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

'
'
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

'
'
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

'
'
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