案例/模板说明
机械臂主控主函数说明
机械臂主控的 api 调用示例在 Main_example 子程序中。
以下为机械臂主控模板代码，注意对工控机返回的 err_code 进行判断。
轨迹移动同步模板
1 CALL: xyzConnectSocket() ; 连接工控机
2 (* "" *)
3 flow_name := "traj_sync.t" ;
4 CALL: err_code := xyzSwitchFlow(flow_name) ; 切换任务为轨迹移动同步
5 IF err_code <> 0 THEN
6 MESSAGE ("Failed") ;
7 ENDPROG ;
8 END_IF ;
9 (* "" *)
10 ws_id := 0 ;
11 item_codename := "item1" ;
12 CALL: err_code := xyzSwitchItem(ws_id, item_codename) ; 切换工件
13 IF err_code <> 0 THEN
14 MESSAGE ("Failed") ;
15 ENDPROG ;
16 END_IF ;
17 (* "" *)
18 io.setDOut(1, false) ; io 初始化
19 MJOINT (POINTJ(0, 0, 0, 0, -90, 0), v50, fine, tool0) ; 机械臂回到初始位置
20 (* "" *)
21 (* "" *)
22 WHILE true DO
23 ws_id := 0 ;
24 CALL: err_code := xyzReqPickPlace(ws_id) ; 请求计算注册抓取
25 IF err_code <> 0 THEN
26 MESSAGE ("Failed") ;
27 ENDPROG ;
28 END_IF ;
29 (* "" *)
30 ws_id := 0 ;
31 CALL: err_code,num,pipeline_num,register_num := xyzGetPickIn(ws_id) ; 获取抓取入筐轨迹
32 IF err_code <> 0 THEN
33 MESSAGE ("Failed") ;
34 ENDPROG ;
35 END_IF ;
36 IF num < 1 THEN
37 GOTO clear_tote_exit ; 无可抓取工件，退出
38 END_IF ;
39 CALL: xyzExecutePickInTraj(num) ; 执行抓取入筐轨迹
40 io.setDOut(1, true) ; 抓取工件
41 (* "" *)
42 ws_id := 0 ;
43 CALL: err_code,num,pipeline_num,register_num := xyzGetPickOut(ws_id) ; 获取抓取出筐轨迹
44 IF err_code <> 0 THEN
45 MESSAGE ("Failed") ;
46 ENDPROG ;
47 END_IF ;
48 CALL: xyzExecutePickOutTraj(num) ; 执行抓取出筐轨迹
49 (* "" *)
50 ws_id := 0 ;
51 CALL: err_code,num,pipeline_num,register_num := xyzGetPlaceIn(ws_id) ; 获取放置入筐轨迹
52 IF err_code <> 0 THEN
53 MESSAGE ("Failed") ;
54 ENDPROG ;
55 END_IF ;
56 CALL: xyzExecuteTraj(num) ; 执行放置入筐轨迹
57 io.setDOut(1, false) ; 放置工件
58 (* "" *)
59 ws_id := 0 ;
60 CALL: err_code,num,pipeline_num,register_num := xyzGetPlaceOut(ws_id) ; 获取放置出筐轨迹
61 IF err_code <> 0 THEN
62 MESSAGE ("Failed") ;
63 ENDPROG ;
64 END_IF ;
65 CALL: xyzExecuteTraj(num) ; 执行放置出筐轨迹
66 (* "" *)
67 END_WHILE ;
68 LABEL clear_tote_exit :
轨迹移动异步模板
1 CALL: xyzConnectSocket() ; 连接工控机
2 (* "" *)
3 flow_name := "traj_async.t" ;
4 CALL: err_code := xyzSwitchFlow(flow_name) ; 切换任务为轨迹移动异步
5 IF err_code <> 0 THEN
6 MESSAGE ("Failed") ;
7 ENDPROG ;
8 END_IF ;
9 (* "" *)
10 ws_id := 0 ;
11 item_codename := "item1" ;
12 CALL: err_code := xyzSwitchItem(ws_id, item_codename) ; 切换工件
13 IF err_code <> 0 THEN
14 MESSAGE ("Failed") ;
15 ENDPROG ;
16 END_IF ;
17 (* "" *)
18 io.setDOut(1, false) ; io 初始化
19 MJOINT (POINTJ(0, 0, 0, 0, -90, 0), v50, fine, tool0) ; 机械臂回到初始位置
20 WHILE true DO
21 ws_id := 0 ;
22 CALL: err_code := xyzReqPickPlace(ws_id) ; 请求计算注册抓取
23 IF err_code <> 0 THEN
24 MESSAGE ("Failed") ;
25 ENDPROG ;
26 END_IF ;
27 (* "" *)
28 ws_id := 0 ;
29 CALL: err_code,num,pipeline_num,register_num := xyzGetPickIn(ws_id) ; 获取抓取入筐轨迹
30 IF err_code <> 0 THEN
31 MESSAGE ("Failed") ;
32 ENDPROG ;
33 END_IF ;
34 IF num < 1 THEN
35 GOTO clear_tote_exit ; 无可抓取工件，退出
36 END_IF ;
37 CALL: xyzExecutePickInTraj(num) ; 执行抓取入筐轨迹
38 io.setDOut(1, true) ;
39 (* "" *)
40 ws_id := 0 ;
41 CALL: err_code,num,pipeline_num,register_num := xyzGetPickOut(ws_id) ; 获取抓取出筐轨迹
42 IF err_code <> 0 THEN
43 MESSAGE ("Failed") ;
44 ENDPROG ;
45 END_IF ;
46 CALL: xyzExecutePickOutTraj(num) ; 执行抓取出筐轨迹
47 (* "" *)
48 ws_id := 0 ;
49 CALL: err_code := xyzReqPickPlace(ws_id) ; 提前请求计算下一次注册放置
50 IF err_code <> 0 THEN
51 MESSAGE ("Failed") ;
52 ENDPROG ;
53 END_IF ;
54 (* "" *)
55 ws_id := 0 ;
56 CALL: err_code,num,pipeline_num,register_num := xyzGetPlaceIn(ws_id) ; 获取放置入筐轨迹
57 IF err_code <> 0 THEN
58 MESSAGE ("Failed") ;
59 ENDPROG ;
60 END_IF ;
61 CALL: xyzExecuteTraj(num) ; 执行放置入筐轨迹
62 io.setDOut(1, false) ; 放置工件
63 (* "" *)
64 ws_id := 0 ;
65 CALL: err_code,num,pipeline_num,register_num := xyzGetPlaceOut(ws_id) ; 获取放置出筐轨迹
66 IF err_code <> 0 THEN
67 MESSAGE ("Failed") ;
68 ENDPROG ;
69 END_IF ;
70 CALL: xyzExecuteTraj(num) ; 执行放置出筐轨迹
71 END_WHILE ;
72 LABEL clear_tote_exit :
73 (* "" *)
座标移动基础模板
1 CALL: xyzConnectSocket() ; 连接工控机
2 (* "" *)
3 flow_name := "cart_basic.t" ;
4 CALL: err_code := xyzSwitchFlow(flow_name) ; 切换任务为座标移动基础任务
5 IF err_code <> 0 THEN
6 MESSAGE ("Failed") ;
7 ENDPROG ;
8 END_IF ;
9 (* "" *)
10 ws_id := 0 ;
11 item_codename := "item1" ;
12 CALL: err_code := xyzSwitchItem(ws_id, item_codename) ; 切换工件
13 IF err_code <> 0 THEN
14 MESSAGE ("Failed") ;
15 ENDPROG ;
16 END_IF ;
17 (* "" *)
18 io.setDOut(1, false) ; io 初始化
19 MJOINT (POINTJ(0, 0, 0, 0, -90, 0), v50, fine, tool0) ; 机械臂回到初始位置
20 (* "" *)
21 is_eye_in_hand := true ; 是否为眼在手上
22 WHILE true DO
23 IF is_eye_in_hand = true THEN 如果眼在手上
24 (* "move to scan pose when eye is in hand" *) 机械臂移动到拍照位置
25 MLIN (POINTC(476.45, -0.14, 768.17, 180, 0, 180, CFG1), v500, fine, tool0) ;
26 CALL: err_code := xyzSendCurrentCartPose() ; 发送当前位姿
27 IF err_code <> 0 THEN
28 MESSAGE ("Failed") ;
29 ENDPROG ;
30 END_IF ;
31 (* "" *)
32 END_IF ;
33 (* "" *)
34 ws_id := 0 ;
35 CALL: err_code,grasp_pose_token := xyzReqGraspPose(ws_id) ; 请求抓取位姿
36 IF err_code <> 0 THEN
37 MESSAGE ("Failed") ;
38 ENDPROG ;
39 END_IF ;
40 获取抓取位姿
41 CALL: err_code,grasp_pose,num,pipeline_num,register_num := xyzGetGraspPose(grasp_pose_token) ;
42 IF err_code <> 0 THEN
43 MESSAGE ("Failed") ;
44 ENDPROG ;
45 END_IF ;
46 (* "" *)
47 IF num < 1 THEN
48 MESSAGE ("no grasp pose, continue requesting next grasp pose") ; 无可抓取工件，继续请求下一个抓取位姿
49 TIMERSTART (timer1, 5) ;
50 WAIT (TIMERQ(timer1)) ; 等待 5 秒
51 CONTINUE ;
52 END_IF ;
53 (* "" *)
54 MLIN (grasp_pose, v500, fine, tool0) ; 机械臂移动到抓取位姿
55 io.setDOut(1, true) ; 抓取工件
56 (* "move to place pose" *) 机械臂移动到放置位姿
57 MLIN (POINTC(476.45, -0.14, 768.17, 180, 0, 180, CFG1), v500, fine, tool0) ;
58 io.setDOut(1, false) ; 放置工件
59 (* "" *)
60 END_WHILE ;
61 (* "" *)
座标移动二次定位模板
1 CALL: xyzConnectSocket() ; 连接工控机
2 (* "" *)
3 flow_name := "cart_repo.t" ;
4 CALL: err_code := xyzSwitchFlow(flow_name) ; 切换任务为座标移动二次定位任务
5 IF err_code <> 0 THEN
6 MESSAGE ("Failed") ;
7 ENDPROG ;
8 END_IF ;
9 (* "" *)
10 io.setDOut(1, false) ; io 初始化
11 io.setDOut(2, false) ;
12 (* "move to home" *)
13 MJOINT (POINTJ(0, 0, 0, 0, -90, 0), v50, fine, tool0) ; 机械臂回到初始位置
14 (* "" *)
15 (* "camera1 rough position" *) 相机1 粗定位
16 ws_id := 0 ;
17 item_codename := "item1" ;
18 CALL: err_code := xyzSwitchItem(ws_id, item_codename) ; 切换工件
19 IF err_code <> 0 THEN
20 MESSAGE ("Failed") ;
21 ENDPROG ;
22 END_IF ;
23 (* "" *)
24 ws_id := 0 ;
25 CALL: err_code,grasp_pose_token := xyzReqGraspPose(ws_id) ; 请求粗抓取位姿
26 IF err_code <> 0 THEN
27 MESSAGE ("Failed") ;
28 ENDPROG ;
29 END_IF ;
30 (* "" *)
31 WHILE true DO
32 (* "camera1 get rough position" *) 相机1 获取粗抓取位姿
33 CALL: err_code,rough_grasp_pose,rough_grasp_pose_num,rough_pipeline_num,rough_register_num := xyzGetGraspPose(grasp_pose_token) ;
34 IF err_code <> 0 THEN
35 MESSAGE ("Failed") ;
36 ENDPROG ;
37 END_IF ;
38 (* "" *)
39 IF rough_grasp_pose_num < 1 THEN 如果无可抓取工件，抓隔板
40 ws_id := 0 ;
41 item_codename := "board" ;
42 CALL: err_code := xyzSwitchItem(ws_id, item_codename) ; 切换工件为隔板
43 IF err_code <> 0 THEN
44 MESSAGE ("Failed") ;
45 ENDPROG ;
46 END_IF ;
47 ws_id := 0 ;
48 CALL: err_code,board_grasp_pose_token := xyzReqGraspPose(ws_id) ; 请求隔板抓取位姿
49 IF err_code <> 0 THEN
50 MESSAGE ("Failed") ;
51 ENDPROG ;
52 END_IF ;
53 (* "camera1 grasp board" *) 相机1 抓取隔板
54 CALL: err_code,board_grasp_pose,board_grasp_pose_num,board_pipeline_num,board_register_num := xyzGetGraspPose(board_grasp_pose_token) ;
55 IF err_code <> 0 THEN
56 MESSAGE ("Failed") ;
57 ENDPROG ;
58 END_IF ;
59 IF board_grasp_pose_num < 1 THEN 如果无可抓取隔板
60 (* "no board, you can change tote" *)
61 ENDPROG ;
62 END_IF ;
63 MLIN (board_grasp_pose, v500, fine, tool0) ; 机械臂移动到隔板抓取位姿
64 io.setDOut(1, true) ; 抓取隔板
65 (* "move to board place pose" *) 机械臂移动到隔板放置位姿
66 MLIN (POINTC(476.45, -0.14, 768.17, 180, 0, 180, CFG1), v500, fine, tool0) ;
67 io.setDOut(1, false) ; 放置隔板
68 (* "camera1 rough position" *) 相机1 粗定位
69 ws_id := 0 ;
70 item_codename := "item1" ;
71 CALL: err_code := xyzSwitchItem(ws_id, item_codename) ; 切换工件
72 IF err_code <> 0 THEN
73 MESSAGE ("Failed") ;
74 ENDPROG ;
75 END_IF ;
76 ws_id := 0 ;
77 CALL: err_code,grasp_pose_token := xyzReqGraspPose(ws_id) ; 请求粗抓取位姿
78 IF err_code <> 0 THEN
79 MESSAGE ("Failed") ;
80 ENDPROG ;
81 END_IF ;
82 CONTINUE ;
83 (* "" *)
84 END_IF ;
85 (* "item exists, camera2 reposition" *) 有可抓取工件，相机2 二次定位
86 (* "move to rough grasp pose" *) 机械臂移动到粗抓取位姿
87 MLIN (rough_grasp_pose, v500, fine, tool0) ;
88 CALL: err_code := xyzSendCurrentCartPose() ; 发送当前位姿
89 IF err_code <> 0 THEN
90 MESSAGE ("Failed") ;
91 ENDPROG ;
92 END_IF ;
93 (* "" *)
94 ws_id := 1 ;
95 item_codename := "item1" ;
96 CALL: err_code := xyzSwitchItem(ws_id, item_codename) ; 切换工件
97 IF err_code <> 0 THEN
98 MESSAGE ("Failed") ;
99 ENDPROG ;
100 END_IF ;
101 ws_id := 1 ;
102 CALL: err_code,grasp_pose_token := xyzReqGraspPose(ws_id) ; 请求精确抓取位姿
103 IF err_code <> 0 THEN
104 MESSAGE ("Failed") ;
105 ENDPROG ;
106 END_IF ;
107 (* "" *) 获取精确抓取位姿
108 CALL: err_code,fine_grasp_pose,fine_grasp_pose_num,fine_pipeline_num,fine_register_num := xyzGetGraspPose(grasp_pose_token) ;
109 IF err_code <> 0 THEN
110 MESSAGE ("Failed") ;
111 ENDPROG ;
112 END_IF ;
113 IF fine_grasp_pose_num < 1 THEN 如果无可抓取工件，退出
114 ENDPROG ;
115 END_IF ;
116 MLIN (fine_grasp_pose, v500, fine, tool0) ; 机械臂移动到精确抓取位姿
117 io.setDOut(2, true) ; 抓取工件
118 (* "move to item place pose" *) 机械臂移动到放置位姿
119 MLIN (POINTC(476.45, -0.14, 768.17, 180, 0, 180, CFG1), v500, fine, tool0) ;
120 io.setDOut(2, false) ; 放置工件
121 (* "" *)
122 (* "camera1 rough position" *) 相机1 粗定位
123 ws_id := 0 ;
124 item_codename := "item1" ;
125 CALL: err_code := xyzSwitchItem(ws_id, item_codename) ; 切换工件
126 IF err_code <> 0 THEN
127 MESSAGE ("Failed") ;
128 ENDPROG ;
129 END_IF ;
130 ws_id := 0 ;
131 CALL: err_code,grasp_pose_token := xyzReqGraspPose(ws_id) ; 请求粗抓取位姿
132 IF err_code <> 0 THEN
133 MESSAGE ("Failed") ;
134 ENDPROG ;
135 END_IF ;
136 (* "" *)
137 (* "" *)
138 END_WHILE ;
139 (* "" *)
140 (* "" *)