案例/模板说明 ============================ 机械臂主控主函数说明 ++++++++++++++++++++++++++++++++ 机械臂主控的 api 调用示例在 Main_example 子程序中。 以下为机械臂主控模板代码，注意对工控机返回的 err_code 进行判断。 轨迹移动同步模板 -------------------------------- .. code-block:: :linenos: CALL: xyzConnectSocket() ; 连接工控机 (* "" *) flow_name := "traj_sync.t" ; CALL: err_code := xyzSwitchFlow(flow_name) ; 切换任务为轨迹移动同步 IF err_code <> 0 THEN MESSAGE ("Failed") ; ENDPROG ; END_IF ; (* "" *) ws_id := 0 ; item_codename := "item1" ; CALL: err_code := xyzSwitchItem(ws_id, item_codename) ; 切换工件 IF err_code <> 0 THEN MESSAGE ("Failed") ; ENDPROG ; END_IF ; (* "" *) io.setDOut(1, false) ; io 初始化 MJOINT (POINTJ(0, 0, 0, 0, -90, 0), v50, fine, tool0) ; 机械臂回到初始位置 (* "" *) (* "" *) WHILE true DO ws_id := 0 ; CALL: err_code := xyzReqPickPlace(ws_id) ; 请求计算注册抓取 IF err_code <> 0 THEN MESSAGE ("Failed") ; ENDPROG ; END_IF ; (* "" *) ws_id := 0 ; CALL: err_code,num,pipeline_num,register_num := xyzGetPickIn(ws_id) ; 获取抓取入筐轨迹 IF err_code <> 0 THEN MESSAGE ("Failed") ; ENDPROG ; END_IF ; IF num < 1 THEN GOTO clear_tote_exit ; 无可抓取工件，退出 END_IF ; CALL: xyzExecutePickInTraj(num) ; 执行抓取入筐轨迹 io.setDOut(1, true) ; 抓取工件 (* "" *) ws_id := 0 ; CALL: err_code,num,pipeline_num,register_num := xyzGetPickOut(ws_id) ; 获取抓取出筐轨迹 IF err_code <> 0 THEN MESSAGE ("Failed") ; ENDPROG ; END_IF ; CALL: xyzExecutePickOutTraj(num) ; 执行抓取出筐轨迹 (* "" *) ws_id := 0 ; CALL: err_code,num,pipeline_num,register_num := xyzGetPlaceIn(ws_id) ; 获取放置入筐轨迹 IF err_code <> 0 THEN MESSAGE ("Failed") ; ENDPROG ; END_IF ; CALL: xyzExecuteTraj(num) ; 执行放置入筐轨迹 io.setDOut(1, false) ; 放置工件 (* "" *) ws_id := 0 ; CALL: err_code,num,pipeline_num,register_num := xyzGetPlaceOut(ws_id) ; 获取放置出筐轨迹 IF err_code <> 0 THEN MESSAGE ("Failed") ; ENDPROG ; END_IF ; CALL: xyzExecuteTraj(num) ; 执行放置出筐轨迹 (* "" *) END_WHILE ; LABEL clear_tote_exit : 轨迹移动异步模板 -------------------------------- .. code-block:: :linenos: CALL: xyzConnectSocket() ; 连接工控机 (* "" *) flow_name := "traj_async.t" ; CALL: err_code := xyzSwitchFlow(flow_name) ; 切换任务为轨迹移动异步 IF err_code <> 0 THEN MESSAGE ("Failed") ; ENDPROG ; END_IF ; (* "" *) ws_id := 0 ; item_codename := "item1" ; CALL: err_code := xyzSwitchItem(ws_id, item_codename) ; 切换工件 IF err_code <> 0 THEN MESSAGE ("Failed") ; ENDPROG ; END_IF ; (* "" *) io.setDOut(1, false) ; io 初始化 MJOINT (POINTJ(0, 0, 0, 0, -90, 0), v50, fine, tool0) ; 机械臂回到初始位置 WHILE true DO ws_id := 0 ; CALL: err_code := xyzReqPickPlace(ws_id) ; 请求计算注册抓取 IF err_code <> 0 THEN MESSAGE ("Failed") ; ENDPROG ; END_IF ; (* "" *) ws_id := 0 ; CALL: err_code,num,pipeline_num,register_num := xyzGetPickIn(ws_id) ; 获取抓取入筐轨迹 IF err_code <> 0 THEN MESSAGE ("Failed") ; ENDPROG ; END_IF ; IF num < 1 THEN GOTO clear_tote_exit ; 无可抓取工件，退出 END_IF ; CALL: xyzExecutePickInTraj(num) ; 执行抓取入筐轨迹 io.setDOut(1, true) ; (* "" *) ws_id := 0 ; CALL: err_code,num,pipeline_num,register_num := xyzGetPickOut(ws_id) ; 获取抓取出筐轨迹 IF err_code <> 0 THEN MESSAGE ("Failed") ; ENDPROG ; END_IF ; CALL: xyzExecutePickOutTraj(num) ; 执行抓取出筐轨迹 (* "" *) ws_id := 0 ; CALL: err_code := xyzReqPickPlace(ws_id) ; 提前请求计算下一次注册放置 IF err_code <> 0 THEN MESSAGE ("Failed") ; ENDPROG ; END_IF ; (* "" *) ws_id := 0 ; CALL: err_code,num,pipeline_num,register_num := xyzGetPlaceIn(ws_id) ; 获取放置入筐轨迹 IF err_code <> 0 THEN MESSAGE ("Failed") ; ENDPROG ; END_IF ; CALL: xyzExecuteTraj(num) ; 执行放置入筐轨迹 io.setDOut(1, false) ; 放置工件 (* "" *) ws_id := 0 ; CALL: err_code,num,pipeline_num,register_num := xyzGetPlaceOut(ws_id) ; 获取放置出筐轨迹 IF err_code <> 0 THEN MESSAGE ("Failed") ; ENDPROG ; END_IF ; CALL: xyzExecuteTraj(num) ; 执行放置出筐轨迹 END_WHILE ; LABEL clear_tote_exit : (* "" *) 座标移动基础模板 -------------------------------- .. code-block:: :linenos: CALL: xyzConnectSocket() ; 连接工控机 (* "" *) flow_name := "cart_basic.t" ; CALL: err_code := xyzSwitchFlow(flow_name) ; 切换任务为座标移动基础任务 IF err_code <> 0 THEN MESSAGE ("Failed") ; ENDPROG ; END_IF ; (* "" *) ws_id := 0 ; item_codename := "item1" ; CALL: err_code := xyzSwitchItem(ws_id, item_codename) ; 切换工件 IF err_code <> 0 THEN MESSAGE ("Failed") ; ENDPROG ; END_IF ; (* "" *) io.setDOut(1, false) ; io 初始化 MJOINT (POINTJ(0, 0, 0, 0, -90, 0), v50, fine, tool0) ; 机械臂回到初始位置 (* "" *) is_eye_in_hand := true ; 是否为眼在手上 WHILE true DO IF is_eye_in_hand = true THEN 如果眼在手上 (* "move to scan pose when eye is in hand" *) 机械臂移动到拍照位置 MLIN (POINTC(476.45, -0.14, 768.17, 180, 0, 180, CFG1), v500, fine, tool0) ; CALL: err_code := xyzSendCurrentCartPose() ; 发送当前位姿 IF err_code <> 0 THEN MESSAGE ("Failed") ; ENDPROG ; END_IF ; (* "" *) END_IF ; (* "" *) ws_id := 0 ; CALL: err_code,grasp_pose_token := xyzReqGraspPose(ws_id) ; 请求抓取位姿 IF err_code <> 0 THEN MESSAGE ("Failed") ; ENDPROG ; END_IF ; 获取抓取位姿 CALL: err_code,grasp_pose,num,pipeline_num,register_num := xyzGetGraspPose(grasp_pose_token) ; IF err_code <> 0 THEN MESSAGE ("Failed") ; ENDPROG ; END_IF ; (* "" *) IF num < 1 THEN MESSAGE ("no grasp pose, continue requesting next grasp pose") ; 无可抓取工件，继续请求下一个抓取位姿 TIMERSTART (timer1, 5) ; WAIT (TIMERQ(timer1)) ; 等待 5 秒 CONTINUE ; END_IF ; (* "" *) MLIN (grasp_pose, v500, fine, tool0) ; 机械臂移动到抓取位姿 io.setDOut(1, true) ; 抓取工件 (* "move to place pose" *) 机械臂移动到放置位姿 MLIN (POINTC(476.45, -0.14, 768.17, 180, 0, 180, CFG1), v500, fine, tool0) ; io.setDOut(1, false) ; 放置工件 (* "" *) END_WHILE ; (* "" *) 座标移动二次定位模板 -------------------------------- .. code-block:: :linenos: CALL: xyzConnectSocket() ; 连接工控机 (* "" *) flow_name := "cart_repo.t" ; CALL: err_code := xyzSwitchFlow(flow_name) ; 切换任务为座标移动二次定位任务 IF err_code <> 0 THEN MESSAGE ("Failed") ; ENDPROG ; END_IF ; (* "" *) io.setDOut(1, false) ; io 初始化 io.setDOut(2, false) ; (* "move to home" *) MJOINT (POINTJ(0, 0, 0, 0, -90, 0), v50, fine, tool0) ; 机械臂回到初始位置 (* "" *) (* "camera1 rough position" *) 相机1 粗定位 ws_id := 0 ; item_codename := "item1" ; CALL: err_code := xyzSwitchItem(ws_id, item_codename) ; 切换工件 IF err_code <> 0 THEN MESSAGE ("Failed") ; ENDPROG ; END_IF ; (* "" *) ws_id := 0 ; CALL: err_code,grasp_pose_token := xyzReqGraspPose(ws_id) ; 请求粗抓取位姿 IF err_code <> 0 THEN MESSAGE ("Failed") ; ENDPROG ; END_IF ; (* "" *) WHILE true DO (* "camera1 get rough position" *) 相机1 获取粗抓取位姿 CALL: err_code,rough_grasp_pose,rough_grasp_pose_num,rough_pipeline_num,rough_register_num := xyzGetGraspPose(grasp_pose_token) ; IF err_code <> 0 THEN MESSAGE ("Failed") ; ENDPROG ; END_IF ; (* "" *) IF rough_grasp_pose_num < 1 THEN 如果无可抓取工件，抓隔板 ws_id := 0 ; item_codename := "board" ; CALL: err_code := xyzSwitchItem(ws_id, item_codename) ; 切换工件为隔板 IF err_code <> 0 THEN MESSAGE ("Failed") ; ENDPROG ; END_IF ; ws_id := 0 ; CALL: err_code,board_grasp_pose_token := xyzReqGraspPose(ws_id) ; 请求隔板抓取位姿 IF err_code <> 0 THEN MESSAGE ("Failed") ; ENDPROG ; END_IF ; (* "camera1 grasp board" *) 相机1 抓取隔板 CALL: err_code,board_grasp_pose,board_grasp_pose_num,board_pipeline_num,board_register_num := xyzGetGraspPose(board_grasp_pose_token) ; IF err_code <> 0 THEN MESSAGE ("Failed") ; ENDPROG ; END_IF ; IF board_grasp_pose_num < 1 THEN 如果无可抓取隔板 (* "no board, you can change tote" *) ENDPROG ; END_IF ; MLIN (board_grasp_pose, v500, fine, tool0) ; 机械臂移动到隔板抓取位姿 io.setDOut(1, true) ; 抓取隔板 (* "move to board place pose" *) 机械臂移动到隔板放置位姿 MLIN (POINTC(476.45, -0.14, 768.17, 180, 0, 180, CFG1), v500, fine, tool0) ; io.setDOut(1, false) ; 放置隔板 (* "camera1 rough position" *) 相机1 粗定位 ws_id := 0 ; item_codename := "item1" ; CALL: err_code := xyzSwitchItem(ws_id, item_codename) ; 切换工件 IF err_code <> 0 THEN MESSAGE ("Failed") ; ENDPROG ; END_IF ; ws_id := 0 ; CALL: err_code,grasp_pose_token := xyzReqGraspPose(ws_id) ; 请求粗抓取位姿 IF err_code <> 0 THEN MESSAGE ("Failed") ; ENDPROG ; END_IF ; CONTINUE ; (* "" *) END_IF ; (* "item exists, camera2 reposition" *) 有可抓取工件，相机2 二次定位 (* "move to rough grasp pose" *) 机械臂移动到粗抓取位姿 MLIN (rough_grasp_pose, v500, fine, tool0) ; CALL: err_code := xyzSendCurrentCartPose() ; 发送当前位姿 IF err_code <> 0 THEN MESSAGE ("Failed") ; ENDPROG ; END_IF ; (* "" *) ws_id := 1 ; item_codename := "item1" ; CALL: err_code := xyzSwitchItem(ws_id, item_codename) ; 切换工件 IF err_code <> 0 THEN MESSAGE ("Failed") ; ENDPROG ; END_IF ; ws_id := 1 ; CALL: err_code,grasp_pose_token := xyzReqGraspPose(ws_id) ; 请求精确抓取位姿 IF err_code <> 0 THEN MESSAGE ("Failed") ; ENDPROG ; END_IF ; (* "" *) 获取精确抓取位姿 CALL: err_code,fine_grasp_pose,fine_grasp_pose_num,fine_pipeline_num,fine_register_num := xyzGetGraspPose(grasp_pose_token) ; IF err_code <> 0 THEN MESSAGE ("Failed") ; ENDPROG ; END_IF ; IF fine_grasp_pose_num < 1 THEN 如果无可抓取工件，退出 ENDPROG ; END_IF ; MLIN (fine_grasp_pose, v500, fine, tool0) ; 机械臂移动到精确抓取位姿 io.setDOut(2, true) ; 抓取工件 (* "move to item place pose" *) 机械臂移动到放置位姿 MLIN (POINTC(476.45, -0.14, 768.17, 180, 0, 180, CFG1), v500, fine, tool0) ; io.setDOut(2, false) ; 放置工件 (* "" *) (* "camera1 rough position" *) 相机1 粗定位 ws_id := 0 ; item_codename := "item1" ; CALL: err_code := xyzSwitchItem(ws_id, item_codename) ; 切换工件 IF err_code <> 0 THEN MESSAGE ("Failed") ; ENDPROG ; END_IF ; ws_id := 0 ; CALL: err_code,grasp_pose_token := xyzReqGraspPose(ws_id) ; 请求粗抓取位姿 IF err_code <> 0 THEN MESSAGE ("Failed") ; ENDPROG ; END_IF ; (* "" *) (* "" *) END_WHILE ; (* "" *) (* "" *)