案例/模板说明 ============================ 机械臂主控模板说明 ++++++++++++++++++++++++++++++++ 以下为机械臂主控模板代码，包含坐标移动基础模板, 坐标移动二次定位模板，轨迹移动同步模板以及轨迹移动异步模板。 关于模板中的 API 可以查阅 Denso 的 “API 说明” 部分。 **请注意模板函数并不能直接运行，请一定根据项目现场环境和流程需求进行修改。** 坐标移动基础模板 ------------------------------------- .. code-block:: 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 坐标移动二次定位模板 -------------------------------------- .. code-block:: 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 轨迹移动同步模板 ------------------------------------- .. code-block:: 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 轨迹移动异步模板 ------------------------------------- .. code-block:: 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