サンプルプログラム10：MM_S10_Viz_Subtask
プログラム概要
機能 |
本サンプルは2つのプログラムで構成されています。サブプログラム（バックグラウンドプログラム）は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]=(ｵﾝ) ;
10: !trigger Mech-Viz project ;
11: ﾖﾋﾞﾀﾞｼ MM_START_VIZ(2,10) ;
12: !get planned path, 1st argument ;
13: !(1) means getting pose in JPs ;
14: ﾖﾋﾞﾀﾞｼ MM_GET_VIZ(1,51,52,53) ;
15: !check whether planned path has ;
16: !been got from Mech-Viz ;
17: !successfully ;
18: IF (ﾚｼﾞ[53]=2100) THEN ;
19: !save waypoints of the planned ;
20: !path to local variables one ;
21: !by one ;
22: ﾖﾋﾞﾀﾞｼ MM_GET_JPS(1,60,70,80) ;
23: ﾖﾋﾞﾀﾞｼ MM_GET_JPS(2,61,71,81) ;
24: ﾖﾋﾞﾀﾞｼ MM_GET_JPS(3,62,72,82) ;
25: ENDIF ;
26: F[11]=(ｵﾌ) ;上記のコードは、サブプログラムが実行時に以下の処理を行うことを示します。
-
F[11]をｵﾝに設定します。
-
Mech-Vizプロジェクトをトリガーして実行します。
-
計画された経路を取得します。
-
計画された経路を保存します。
-
F[11]をｵﾌに設定します。
これにより、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: ﾕｰｻﾞｻﾞﾋｮｳﾊﾞﾝｺﾞｳ=0 ;
11: !set current tool NO. to 1 ;
12: ﾂｰﾙｻﾞﾋｮｳﾊﾞﾝｺﾞｳ=1 ;
13: !initialize communication ;
14: !parameters(initialization is ;
15: !required only once) ;
16: ﾖﾋﾞﾀﾞｼ MM_INIT_SKT('8','127.0.0.1',50000,5) ;
17: !move to robot home position ;
18:ｶｸｼﾞｸ ｲﾁ[1] 100% ｲﾁｷﾞﾒ ;
19: ｼﾞｯｺｳ MM_S10_SUB ;
20: ﾗﾍﾞﾙ[1:LOOP] ;
21: !move to wait position for ;
22: !picking ;
23:ﾁｮｸｾﾝ ｲﾁ[2] 1000mm/sec ｲﾁｷﾞﾒ ;
24: !wait until subtask program ;
25: !finished ;
26: ﾀｲｷ (F[11]=ｵﾌ) ;
27: !check whether planned path has ;
28: !been got from Mech-Viz ;
29: !successfully ;
30: ﾓｼ ﾚｼﾞ[53]<>2100,ｼﾞｬﾝﾌﾟ ﾗﾍﾞﾙ[99] ;
31: !follow the planned path to pick ;
32: !move to approach waypoint ;
33: !of picking ;
34:ｶｸｼﾞｸ ｲﾁﾚｼﾞ[60] 50% ｲﾁｷﾞﾒ ;
35: !move to picking waypoint ;
36:ｶｸｼﾞｸ ｲﾁﾚｼﾞ[61] 10% ｲﾁｷﾞﾒ ;
37: !add object grasping logic here, ;
38: !such as "DO[1]=ON" ;
39: ｲﾁｼﾞﾃｲｼ ;
40: !move to departure waypoint ;
41: !of picking ;
42:ｶｸｼﾞｸ ｲﾁﾚｼﾞ[62] 50% ｲﾁｷﾞﾒ ;
43: !move to intermediate waypoint ;
44: !of placing, and trigger Mech-Viz ;
45: !project and get planned path in ;
46: !advance ;
47:ｶｸｼﾞｸ ｲﾁ[3] 50% ﾅﾒﾗｶ100 ｻｷｼﾞｯｺｳ 10.0mm,ﾖﾋﾞﾀﾞｼ MM_S10_SUB ;
48: !move to approach waypoint ;
49: !of placing ;
50:ﾁｮｸｾﾝ ｲﾁ[4] 1000mm/sec ｲﾁｷﾞﾒ ﾂｰﾙﾎｾｲ,ｲﾁﾚｼﾞ[2] ;
51: !move to placing waypoint ;
52:ﾁｮｸｾﾝ ｲﾁ[4] 300mm/sec ｲﾁｷﾞﾒ ;
53: !add object releasing logic here, ;
54: !such as "DO[1]=OFF" ;
55: ｲﾁｼﾞﾃｲｼ ;
56: !move to departure waypoint ;
57: !of placing ;
58:ﾁｮｸｾﾝ ｲﾁ[4] 1000mm/sec ｲﾁｷﾞﾒ ﾂｰﾙﾎｾｲ,ｲﾁﾚｼﾞ[2] ;
59: !move back to robot home position ;
60:ｶｸｼﾞｸ ｲﾁ[1] 100% ｲﾁｷﾞﾒ ;
61: ｼﾞｬﾝﾌﾟ ﾗﾍﾞﾙ[1] ;
62: ｼｭｳﾘｮｳ ;
63: ;
64: ﾗﾍﾞﾙ[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: ｲﾁｼﾞﾃｲｼ ;上記のサンプルプログラムの処理流れは、下図の通りです。
下表は主要なコードとその説明です。
| 処理流れ | コートと説明 | ||
|---|---|---|---|
Mech-Vizプロジェクトをトリガーし、計画された経路を取得 |
上記のコートは、メインプログラムがMM_S10_SUBサブプログラムを呼び出し、これによってMech-Vizプロジェクトをトリガーし、計画された経路を取得することを意味します。
|
||
ループで次の経路を事前に計画（把持→次の把持経路を計画→配置） |
上記のコードは、メインプログラムがﾗﾍﾞﾙ[1]で示すコードを繰り返し実行することを意味します。 上記のコードは、ロボットが把持する前に待機点に移動し、サブプログラムの実行が終了するのを待つことを意味します。つまり、F[11]がｵﾝからｵﾌに変わるのを待つことで、計画された経路が取得され、保存されたことを確認します。 上記のコードは、ロボットが計画された経路に従って把持のアプローチ点（ｲﾁﾚｼﾞ[60]）に移動し、次に把持点（ｲﾁﾚｼﾞ[61]）に移動して把持を行い（例えばDO[1]=ON）、その後、把持の離れる点（ｲﾁﾚｼﾞ[62]）に移動することを意味します。 上記のコマンドは、「先実行距離」コマンドを意味します。詳細な説明は以下の通りです。
上記のコードは、ロボットがｲﾁ[3]を中心とした球形範囲に到達した際に、MM_S10_SUBサブプログラムを呼び出すことを意味します。これにより、Mech-Vizプロジェクトが再度実行され、次の計画経路が取得されます。この時点でロボットはすで把持領域外にいるため、ロボットは配置が完了するのを待つことなく、次の把持経路を事前に計画することができます。
上記のコードは、ロボットが配置のアプローチ点（ﾂｰﾙﾎｾｲ,ｲﾁﾚｼﾞ[2]）、配置点（ｲﾁ[4]）に移動して配置（例えばDO[1] = OFF）を行い、その後、配置の離れ点（ﾂｰﾙﾎｾｲ,ｲﾁﾚｼﾞ[2]）、Home位置の順に移動することを意味します。 |