サンプルプログラム8：MM_S8_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_S8_Viz_Subtask) ;
6: !Mech-Mind, 2023-12-25 ;
7: !-------------------------------- ;
8: ;
9: F[10]=(ｵﾝ) ;
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[10]=(ｵﾌ) ;上記のコードは、サブプログラムが実行時に以下の処理を行うことを示します。
-
F[10]をｵﾝに設定します。
-
Mech-Vizプロジェクトをトリガーして実行します。
-
計画された経路を取得します。
-
計画された経路を保存します。
-
F[10]をｵﾌに設定します。
これにより、F[10]の値に基づいてサブプログラムの実行が完了したかどうかを判断できます。
以下は、メインプログラムのコードとその説明です。
| メインプログラム内で計画された経路に基づいて把持または配置を実行するコードは「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_S8_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_S8_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[10]=ｵﾌ) ;
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 ;
45:ｶｸｼﾞｸ ｲﾁ[3] 50% ﾅﾒﾗｶ100 ;
46: !move to approach waypoint ;
47: !of placing ;
48:ﾁｮｸｾﾝ ｲﾁ[4] 1000mm/sec ｲﾁｷﾞﾒ ﾂｰﾙﾎｾｲ,ｲﾁﾚｼﾞ[2] ;
49: !trigger Mech-Viz project and get ;
50: !planned path in advance ;
51: ｼﾞｯｺｳ MM_S8_SUB ;
52: !move to placing waypoint ;
53:ﾁｮｸｾﾝ ｲﾁ[4] 300mm/sec ｲﾁｷﾞﾒ ;
54: !add object releasing logic here, ;
55: !such as "DO[1]=OFF" ;
56: ｲﾁｼﾞﾃｲｼ ;
57: !move to departure waypoint ;
58: !of placing ;
59:ﾁｮｸｾﾝ ｲﾁ[4] 1000mm/sec ｲﾁｷﾞﾒ ﾂｰﾙﾎｾｲ,ｲﾁﾚｼﾞ[2] ;
60: !move back to robot home position ;
61:ｶｸｼﾞｸ ｲﾁ[1] 100% ｲﾁｷﾞﾒ ;
62: ｼﾞｬﾝﾌﾟ ﾗﾍﾞﾙ[1] ;
63: ｼｭｳﾘｮｳ ;
64: ;
65: ﾗﾍﾞﾙ[99:vision error] ;
66: !add error handling logic here ;
67: !according to different ;
68: !error codes ;
69: !e.g.: status=2038 means no ;
70: !point cloud in ROI ;
71: ｲﾁｼﾞﾃｲｼ ;上記のサンプルプログラムの処理流れは、下図の通りです。
下表はメインプログラムの主要なコードとその説明です。
| 処理流れ | コートと説明 | ||
|---|---|---|---|
Mech-Vizプロジェクトをトリガーし、計画された経路を取得 |
上記のコートは、メインプログラムがMM_S8_SUBサブプログラムを呼び出し、これによってMech-Vizプロジェクトをトリガーし、計画された経路を取得することを意味します。
|
||
ループで次の経路を事前に計画（把持→次の把持経路を計画→配置） |
上記のコードは、メインプログラムがﾗﾍﾞﾙ[1]で示すコードを繰り返し実行することを意味します。 上記のコードは、ロボットが把持する前に待機点に移動し、サブプログラムの実行が終了するのを待つことを意味します。つまり、F[10]がｵﾝからｵﾌに変わるのを待つことで、計画された経路が取得され、保存されたことを確認します。 上記のコードは、ロボットが計画された経路に従って把持のアプローチ点（ｲﾁﾚｼﾞ[60]）に移動し、次に把持点（ｲﾁﾚｼﾞ[61]）に移動して把持を行い（例えばDO[1]=ON）、その後、把持の離れる点（ｲﾁﾚｼﾞ[62]）、配置の中間点（ﾚｼﾞ[3]）、配置のアプローチ点（ﾂｰﾙﾎｾｲ,ｲﾁﾚｼﾞ[2]）の順に移動することを意味します。 上記のコードは、メインプログラムがMM_S8_SUBサブプログラムを再度呼び出し、これによってMech-Vizプロジェクトを再度トリガーして計画された経路を取得することを意味します。ここではロボットがすでに配置領域にいるため、ロボットは配置が完了するのを待つことなく、次の把持経路を事前に計画することができます。 上記のコードは、ロボットが配置点（ｲﾁ[4]）に移動して配置を行い（例えばDO[1]=OFF）、その後、配置の離れる点（ﾂｰﾙﾎｾｲ,ｲﾁﾚｼﾞ[2]）、Home点の順に移動することを意味します。 |