样例程序10：MM_S10_Viz_Subtask
程序简介
功能说明 |
本样例由两个程序组成，其中子任务负责触发Mech-Viz工程运行并获取规划路径；主任务负责根据规划路径移动机器人，且在机器人离开抓取区域时触发子任务运行，从而提前规划下一轮路径，以缩短节拍。
|
||
文件路径 |
子任务：Mech-Vision和Mech-Viz软件安装目录下 主任务：Mech-Vision和Mech-Viz软件安装目录下 |
||
所需工程 |
Mech-Vision工程和Mech-Viz工程 |
||
使用前提 |
|
此样例程序仅是示例程序。用户需根据实际情况在此基础上进行修改，请勿直接使用该程序。 |
程序解读
以下为子任务的代码及相关解释说明。
| 在子任务中，触发Mech-Viz工程运行并获取规划路径的代码，与MM_S2_Viz_Basic样例中类似。因此，下文不再重复解释与MM_S2_Viz_Basic样例相同部分的代码（详情请参考MM_S2_Viz_Basic样例说明）。 |
1: !-------------------------------- ;
2: !FUNCTION: run Mech-Viz project ;
3: !and get planned path in subtask ;
4: !(run together with ;
5: !MM_S10_Viz_Subtask) ;
6: !Mech-Mind, 2023-12-25 ;
7: !-------------------------------- ;
8: ;
9: F[11]=(ON) ;
10: !trigger Mech-Viz project ;
11: CALL MM_START_VIZ(2,10) ;
12: !get planned path, 1st argument ;
13: !(1) means getting pose in JPs ;
14: CALL MM_GET_VIZ(1,51,52,53) ;
15: !check whether planned path has ;
16: !been got from Mech-Viz ;
17: !successfully ;
18: IF (R[53]=2100) THEN ;
19: !save waypoints of the planned ;
20: !path to local variables one ;
21: !by one ;
22: CALL MM_GET_JPS(1,60,70,80) ;
23: CALL MM_GET_JPS(2,61,71,81) ;
24: CALL MM_GET_JPS(3,62,72,82) ;
25: ENDIF ;
26: F[11]=(OFF) ;上述代码表示，子任务在运行时，执行以下流程。
-
将F[11]设置为ON。
-
触发Mech-Viz工程运行。
-
获取规划路径。
-
转存规划路径。
-
将F[11]设置为OFF。
如此，根据F[11]的值便可判断子任务是否运行完成。
以下为主任务的代码及相关解释说明。
| 在主任务中，根据规划路径进行抓取与放置的代码，与MM_S2_Viz_Basic样例中类似。因此，下文不再重复解释与MM_S2_Viz_Basic样例相同部分的代码（详情请参考MM_S2_Viz_Basic样例说明）。 |
1: !-------------------------------- ;
2: !FUNCTION: run Mech-Viz project ;
3: !and get planned path in subtask ;
4: !(run together with ;
5: !MM_S10_Sub) ;
6: !Mech-Mind, 2023-12-25 ;
7: !-------------------------------- ;
8: ;
9: !set current uframe NO. to 0 ;
10: UFRAME_NUM=0 ;
11: !set current tool NO. to 1 ;
12: UTOOL_NUM=1 ;
13: !initialize communication ;
14: !parameters(initialization is ;
15: !required only once) ;
16: CALL MM_INIT_SKT('8','127.0.0.1',50000,5) ;
17: !move to robot home position ;
18:J P[1] 100% FINE ;
19: RUN MM_S10_SUB ;
20: LBL[1:LOOP] ;
21: !move to wait position for ;
22: !picking ;
23:L P[2] 1000mm/sec FINE ;
24: !wait until subtask program ;
25: !finished ;
26: WAIT (F[11]=OFF) ;
27: !check whether planned path has ;
28: !been got from Mech-Viz ;
29: !successfully ;
30: IF R[53]<>2100,JMP LBL[99] ;
31: !follow the planned path to pick ;
32: !move to approach waypoint ;
33: !of picking ;
34:J PR[60] 50% FINE ;
35: !move to picking waypoint ;
36:J PR[61] 10% FINE ;
37: !add object grasping logic here, ;
38: !such as "DO[1]=ON" ;
39: PAUSE ;
40: !move to departure waypoint ;
41: !of picking ;
42:J PR[62] 50% FINE ;
43: !move to intermediate waypoint ;
44: !of placing, and trigger Mech-Viz ;
45: !project and get planned path in ;
46: !advance ;
47:J P[3] 50% CNT100 DB 10.0mm,CALL MM_S10_SUB ;
48: !move to approach waypoint ;
49: !of placing ;
50:L P[4] 1000mm/sec FINE Tool_Offset,PR[2] ;
51: !move to placing waypoint ;
52:L P[4] 300mm/sec FINE ;
53: !add object releasing logic here, ;
54: !such as "DO[1]=OFF" ;
55: PAUSE ;
56: !move to departure waypoint ;
57: !of placing ;
58:L P[4] 1000mm/sec FINE Tool_Offset,PR[2] ;
59: !move back to robot home position ;
60:J P[1] 100% FINE ;
61: JMP LBL[1] ;
62: END ;
63: ;
64: LBL[99:vision error] ;
65: !add error handling logic here ;
66: !according to different ;
67: !error codes ;
68: !e.g.: status=2038 means no ;
69: !point cloud in ROI ;
70: PAUSE ;上述样例程序代码对应的流程如下图所示。
下表为主任务核心代码的逻辑解读。
| 流程 | 代码及说明 | ||
|---|---|---|---|
触发Mech-Viz工程运行并获取规划路径 |
上述代码表示，主任务调用MM_S10_SUB子任务，从而触发Mech-Viz工程运行并获取规划路径。 |
||
通过循环（抓取→触发下一轮规划→放置）提前规划下一轮路径 |
上述代码表示，主任务循环执行LBL[1]处的代码。 上述代码表示，机器人移动到抓取前的一个等待点，等待子任务运行完毕（即F[11]从ON变为OFF），从而确保已获取并转存规划路径。 上述代码表示，机器人根据规划的路径先移动到抓取接近点（PR[60]），然后移动到抓取点（PR[61]），进行抓取（例如，DO[1]=ON），再移动到抓取离开点（PR[62]）。
上述代码表示，机器人在到达以P[3]为中心的球形区域时，调用MM_S10_SUB子程序，即再次触发Mech-Viz工程运行并获取规划路径。此处机器人已处于抓取区域外，因此机器人此时可以提前规划下一轮抓取路径，而不必等待放置完成后再规划下一轮抓取路径。
上述代码表示，机器人先依次移动到放置接近点（Tool_Offset,PR[2]）、放置点（P[4]），然后进行放置操作（例如，DO[1]=OFF），然后依次移动到放置离开点（Tool_Offset,PR[2]）、Home点。 |