案例/模板说明

机械臂主控模板说明

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

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

坐标移动基础模板

Sub CartMoveBasic()
    ' S1: 初始化
    Dim err_code As Integer       ' 错误代码
    Dim eye_in_hand As Integer    ' true 表示眼在手上，false 表示眼在手外
    Dim token As Integer          ' 获取请求结果凭证
    Dim home_pose As Joint        ' home 点
    Dim scan_pose As Position     ' 眼在手上时，需要发送的拍照位
    Dim grasp_pose As Position    ' 抓取位姿
    Dim pose_num As Integer       ' 识别出的位姿个数
    Dim pipeline_num As Integer   ' 处理流程编号
    Dim register_num As Integer   ' 抓取序号
    Dim ints_array(5) As Integer  '

    ReSet IO[128]

    ' S2: 连接到工控机
    Call xyzMasterConnect()

    ' 切换任务流图 ，默认被注释掉，如果需求可以取消注释
    ' err_code = xyzSwitchTask("cart_basic.t")
    ' 检查工控机返回的错误码，如果有错误程序将直接停止。可以根据项目需求自定义错误代码处理方式
    ' Call xyzCheckErrorCode(err_code)
    err_code = xyzResetTask()
    Call xyzCheckErrorCode(err_code)

    ' S3: 切换成当前工件
    err_code = xyzSwitchItem(0, "item1")
    Call xyzCheckErrorCode(err_code)

    ' 4: 运动到 home 位
    ' 首先移动到 home 点。注意需要先定义 home_pose 的关节角度值
    ' 或者用使用其他点来替代
    Move P, @[0] home_pose

    ' 如果是眼在机械臂上的应用形式，请把 eye_in_hand 赋值为 true
    eye_in_hand = False
    Do
        ' S5: 眼在机械臂上
        if eye_in_hand = True then
            ' S6: 运动到拍照位姿
            ' 注意需要先定义 scan_pose 的位姿
            ' 或者用使用其他点来替代
            Move L, @[0] scan_pose
            ' S7: 发送拍照位姿
            err_code = xyzSendCurrentCartPose(scan_pose)
            Call xyzCheckErrorCode(err_code)
        End if

        ' S8: 请求抓取点位
        err_code = xyzReqGraspPose(0, token)
        Call xyzCheckErrorCode(err_code)

        ' S9: 获取抓取点位
        err_code = xyzGetGraspPose(token, pose_num, pipeline_num, register_num, grasp_pose, ints_array)
        Call xyzCheckErrorCode(err_code)
        if pose_num < 1 then
            ' 处理没有识别到工件的情况
            Stop
        End if

        ' S10: 运动到抓取点并抓取工件
        ' 需要添加其他路径点作为过渡

        Move L, @[0] grasp_pose
        Set IO[128]

        ' 需要添加其他路径点作为过渡点，使机械臂运动到放置点

        ReSet IO[128]
    Loop While (True)

End Sub

坐标移动二次定位模板

Sub CartMoveRepo()
    ' S1: 初始化
    Dim err_code As Integer     ' 错误代码
    Dim token As Integer        ' 获取请求结果凭证1
    Dim token1 As Integer       ' 获取请求结果凭证2
    Dim home_pose As Joint      ' home 点
    Dim scan_pose As Position   ' 拍照位姿
    Dim grasp_pose As Position  ' 抓取位姿
    Dim pose_num As Integer     ' 识别出的位姿个数
    Dim pipeline_num As Integer ' 处理流程编号
    Dim register_num As Integer ' 抓取序号
    Dim ints_array(5) As Integer

    ReSet IO[128]
    ReSet IO[129]

    ' S2: 连接到工控机
    Call xyzMasterConnect()

    ' 切换任务流图，默认被注释掉，如果需求可以取消注释
    ' err_code = xyzSwitchTask("cart_repo.t")
    ' 检查工控机返回的错误码，如果有错误程序将直接停止。可以根据项目需求自定义错误代码处理方式
    ' Call xyzCheckErrorCode(err_code)
    err_code = xyzResetTask()
    Call xyzCheckErrorCode(err_code)

    ' S3: 机械臂外的相机切换成识别当前工件
    err_code = xyzSwitchItem(0, "item1")
    Call xyzCheckErrorCode(err_code)

    ' S4: 运动到 home 位
    ' 首先移动到 home 点。注意需要先定义 home_pose 的关节角度值
    ' 或者用使用其他点来替代
    Move P, @[0] home_pose

    Do
        ' S5: 请求工件的抓取位姿
        err_code = xyzReqGraspPose(0, token)
        Call xyzCheckErrorCode(err_code)

        ' S6: 获取工件的抓取位姿
        err_code = xyzGetGraspPose(token, pose_num, pipeline_num, register_num, grasp_pose, ints_array)
        Call xyzCheckErrorCode(err_code)

        ' S7: 处理是否识别到工件
        if pose_num < 1 then
            ' 工作空间内没有工件，需要先移除隔板
            ' S15: 没有识别到工件，机械臂外的相机切换识别隔板
            err_code = xyzSwitchItem(0, "board")
            Call xyzCheckErrorCode(err_code)

            ' S16: 请求隔板抓取位姿
            err_code = xyzReqGraspPose(0, token)
            Call xyzCheckErrorCode(err_code)

            ' S17: 获取隔板的抓取位姿
            err_code = xyzGetGraspPose(token, pose_num, pipeline_num, register_num, grasp_pose, ints_array)
            Call xyzCheckErrorCode(err_code)
            if pose_num < 1 then
                PrintDbg "Get board grasp pose failed"
                Stop
            End if

            ' S18: 运动到隔板抓取位姿，并抓取隔板
            ' 注意: 可能需要添加其他路径点作为过渡
            Move L, @[0] grasp_pose

            ' S19: 运动到隔板的放置位姿
            ' 注意: 可能需要添加其他路径点作为过渡
            ' 注意需要定义隔板放置位姿


            ' S20: 机械臂外的相机切换成识别当前工件
            err_code = xyzSwitchItem(0, "item1")
            Call xyzCheckErrorCode(err_code)
        else
            ' S8: 运动到拍照位姿
            ' 注意需要先定义 scan_pose 的位姿
            ' 或者用使用其他点来替代
            Move L, @[0] scan_pose
            ' S9: 发送拍照位姿
            err_code = xyzSendCurrentCartPose(scan_pose)
            Call xyzCheckErrorCode(err_code)

            ' S10: 机械臂上的相机切换成识别当前工件
            err_code = xyzSwitchItem(1, "item1")
            Call xyzCheckErrorCode(err_code)

            ' S11: 请求机械臂上的相机获取工件抓取位姿
            err_code = xyzReqGraspPose(1, token1)
            Call xyzCheckErrorCode(err_code)

            ' S12: 获取工件的抓取位姿
            err_code = xyzGetGraspPose(token1, pose_num, pipeline_num, register_num, grasp_pose, ints_array)
            Call xyzCheckErrorCode(err_code)
            if pose_num < 1 then
                ' 未识别到工件的处理
                PrintDbg "Get object grasp pose failed"
                Stop
            End if

            ' S13: 运动到工件的抓取位姿并进行抓取
            ' 注意: 可能需要添加其他路径点作为过渡

            Move L, @[0] grasp_pose

            ' S14: 运动到工件的放置位姿
            ' 注意：可能需要添加其他路径点作为过渡
            ' 注意：需要定义工件放置位姿

        End if
    Loop While (True)
End Sub

轨迹移动同步模板

Sub TrajMoveSync()
    ' S1: 初始化
    Dim err_code As Integer     ' 错误代码
    Dim vs_id As Integer        ' 视觉服务编号
    Dim pose_num As Integer     ' 轨迹点的个数
    Dim pipeline_num As Integer ' 处理流程编号
    Dim register_num As Integer ' 抓取序号
    Dim home_pose As Joint      ' home 点位
    Dim traj As Variant         ' 路径点数据

    vs_id = 0

    ' S2: 连接到工控机
    Call xyzMasterConnect()

    ' 切换任务流图，默认被注释掉，如果需求可以取消注释
    ' err_code = xyzSwitchTask("traj_sync.t")
    ' Call xyzCheckErrorCode(err_code)
    err_code = xyzResetTask()
    Call xyzCheckErrorCode(err_code)

    ' S3: 切换成当前工件
    err_code = xyzSwitchItem(0, "item1")
    Call xyzCheckErrorCode(err_code)

    ' S4: 运动到 home 位
    ' 首先移动到 home 点。注意需要先定义 home_pose 的关节角度值
    ' 或者用使用其他点来替代
    Move P, @[0] home_pose

    Do
        ' S5: 请求抓取和放置规划
        err_code = xyzReqPickPlace(vs_id)
        Call xyzCheckErrorCode(err_code)

        ' S6: S6: 获取 pick-in 轨迹
        err_code = xyzGetPickin(vs_id, pipeline_num, register_num, pose_num, traj)
        Call xyzCheckErrorCode(err_code)

        ' S7: 判断是否识别到工件
        if pose_num < 1 then
            PrintDbg "tote clear"
            Stop
        End if

        ' S8: 执行 pick-in 轨迹
        Call xyzExecuteTraj(pose_num, traj)

        ' S9: 获取 pick-out 轨迹
        err_code = xyzGetPickout(vs_id, pipeline_num, register_num, pose_num, traj)
        Call xyzCheckErrorCode(err_code)

        ' S10: 执行 pick-out 轨迹
        Call xyzExecuteTraj(pose_num, traj)

        ' S11: 获取 place-in 轨迹
        err_code = xyzGetPlacein(vs_id, pipeline_num, register_num, pose_num, traj)
        Call xyzCheckErrorCode(err_code)

        ' S12: 执行 place-in 轨迹
        Call xyzExecuteTraj(pose_num, traj)

        ' S13: 获取 place-out 轨迹
        err_code = xyzGetPlaceout(vs_id, pipeline_num, register_num, pose_num, traj)
        Call xyzCheckErrorCode(err_code)

        ' S14: 执行 place-out 轨迹
        Call xyzExecuteTraj(pose_num, traj)
    Loop While (True)
End Sub

轨迹移动异步模板

Sub TrajMoveAsync()
    ' S1: 初始化
    Dim err_code As Integer     ' 错误代码
    Dim vs_id As Integer        ' 视觉服务id
    Dim pose_num As Integer     ' 轨迹点的个数
    Dim pipeline_num As Integer ' 处理流程编号
    Dim register_num As Integer ' 抓取序号
    Dim home_pose As Joint      ' 示教的 home 点位
    Dim traj As Variant         ' 路径点数据

    vs_id = 0

    ' S2: 连接到工控机
    Call xyzMasterConnect()

    ' err_code = xyzSwitchTask("traj_async.t")
    ' Call xyzCheckErrorCode(err_code)
    err_code = xyzResetTask()
    Call xyzCheckErrorCode(err_code)

    ' S3: 切换成当前工件
    err_code = xyzSwitchItem(0, "item1")
    Call xyzCheckErrorCode(err_code)

    ' S4: 运动到 home 位
    ' 首先移动到 home 点。注意需要先定义 home_pose 的关节角度值
    ' 或者用使用其他点来替代
    Move P, @[0] home_pose

    ' S5: 请求抓取和放置规划
    err_code = xyzReqPickPlace(vs_id)
    Call xyzCheckErrorCode(err_code)

    Do
        ' S6: 获取 pick-in 轨迹
        err_code = xyzGetPickin(vs_id, pipeline_num, register_num, pose_num, traj)
        Call xyzCheckErrorCode(err_code)

        ' S7: 判断是否识别到工件
        if pose_num < 1 then
            PrintDbg "tote clear"
            Stop
        End if

        ' S8: 执行 pick-in 轨迹
        Call xyzExecuteTraj(pose_num, traj)

        ' S9: 获取 pick-out 轨迹
        err_code = xyzGetPickout(vs_id, pipeline_num, register_num, pose_num, traj)
        Call xyzCheckErrorCode(err_code)

        ' S10: 执行 pick-out 轨迹
        Call xyzExecuteTraj(pose_num, traj)

        ' S11: 请求下一次的抓取和放置规划
        ' 运动到相机视野外，开始请求下一次的抓取和放置规划
        err_code = xyzReqPickPlace(vs_id)
        Call xyzCheckErrorCode(err_code)

        ' S12: 获取本次 place-in 轨迹
        err_code = xyzGetPlacein(vs_id, pipeline_num, register_num, pose_num, traj)
        Call xyzCheckErrorCode(err_code)

        ' S13: 执行本次 place-in 轨迹
        Call xyzExecuteTraj(pose_num, traj)

        ' S14: 获取本次 place-out 轨迹
        err_code = xyzGetPlaceout(vs_id, pipeline_num, register_num, pose_num, traj)
        Call xyzCheckErrorCode(err_code)

        ' S15: 执行本次 place-out 轨迹
        Call xyzExecuteTraj(pose_num, traj)
    Loop While (True)
End Sub