샘플 프로그램8: MM_S8_Viz_Subtask
프로그램 소개
기능 설명 |
이 샘플은 두 개의 프로그램으로 구성되어 있습니다. 그 중, 서브 프로그램(백그라운드 프로그램)은 Mech-Viz프로젝트 실행을 트리거하고 계획된 경로를 가져오는 역할을 합니다. 메인 프로그램(포어그라운드 프로그램)은 계획된 경로에 따라 로봇을 이동시키며, 로봇이 배치할 때 서브 프로그램을 실행하여 다음 라운드 경로를 미리 계획하여 사이클 타임을 단축합니다.
|
||
파일 경로 |
서브 프로그램: Mech-Vision 및 Mech-Viz의 설치 디렉토리로 이동하여 메인 프로그램: Mech-Vision 및 Mech-Viz의 설치 디렉토리로 이동하여 |
||
필요한 프로젝트 |
Mech-Vision와 Mech-Viz 프로젝트 |
||
사용 전제 조건 |
|
|
프로그램 설명
아래는 서브 프로그램의 코드와 관련된 설명입니다.
| 서브 프로그램에서 Mech-Viz 프로젝트 실행을 트리거하고 계획 경로의 코드를 획득합니다. MM_S2_Viz_Basic 샘플과 비슷합니다. 따라서, MM_S8_Viz_Subtask의 MM_S2_Viz_Basic과 일치하는 부분은 다시 설명하지 않습니다. (일치하는 부분에 대한 정보는 MM_S2_Viz_Basic샘플 프로그램 설명을 참조하십시오). |
DEF MM_S8_Sub ( )
;---------------------------------------------------
; FUNCTION: run Mech-Viz project and get planned
; path in subtask (run together with MM_S8_Viz_Subtask)
; Mech-Mind, 2023-12-25
;---------------------------------------------------
$OUT[2000]=FALSE
;initialize communication parameters (initialization is required only once)
MM_Init_Socket("XML_Kuka_MMIND",873,871,60)
LOOP
IF $OUT[2000]==TRUE THEN
;trigger Mech-Viz project
MM_Start_Viz(2,init_jps_a)
;get planned path, 1st argument (1) means getting pose in JPs
MM_Get_VizData(1,pos_num_a,vis_pos_num_a,status_a)
;check whether planned path has been got from Mech-Viz successfully
IF status_a==2100 THEN
;save waypoints of the planned path to local variables one by one
MM_Get_Jps(1,pick_point_a[1],label_a[1],toolid_a[1])
MM_Get_Jps(2,pick_point_a[2],label_a[2],toolid_a[2])
MM_Get_Jps(3,pick_point_a[3],label_a[3],toolid_a[3])
ENDIF
$OUT[2000]=FALSE
ENDIF
ENDLOOP
END위 코드는 서브 프로그램은 실행할 때 먼저 $OUT[2000]이 FALSE로 설정되고 통신 파라미터를 초기화한 후 LOOP 순환을 통해 $OUT[2000]의 값을 계속 모니터링합니다.
-
$OUT[2000]이 TRUE로 설정된 경우, 서브 프로그램은 Mech-Viz 프로젝트 실행을 트리거하고 계획 경로를 획득한 후 $OUT[2000]이 FALSE로 설정됩니다.
-
$OUT[2000]이 FALSE로 설정된 경우, 서브 프로그램은 $OUT[2000]의 값을 계속 판단합니다.
아래는 메인 프로그램의 코드와 관련된 설명입니다.
| 서브 프로그램에서 계획된 경로에 따라 피킹 및 배치의 코드를 실행합니다. MM_S2_Viz_Basic 샘플과 비슷합니다. 따라서, MM_S8_Viz_Subtask의 MM_S2_Viz_Basic과 일치하는 부분은 다시 설명하지 않습니다. (일치하는 부분에 대한 정보는 MM_S2_Viz_Basic샘플 프로그램 설명을 참조하십시오). |
DEF MM_S8_Viz_Subtask ( )
;---------------------------------------------------
; FUNCTION: run Mech-Viz project and get planned
; path in subtask (run together with MM_S8_Sub)
; Mech-Mind, 2023-12-25
;---------------------------------------------------
;set current tool no. to 1
BAS(#TOOL,1)
;set current base no. to 0
BAS(#BASE,0)
;move to robot home position
PTP HOME Vel=100 % DEFAULT
;trigger Mech-Viz project and get planned path
$OUT[2000]=TRUE
LOOP
;move to wait position for picking
LIN pick_wait_point Vel=1 m/s CPDAT6 Tool[1] Base[0]
;wait until subtask program finished
Wait for ($OUT[2000]==FALSE)
;check whether planned path has been got from Mech-Viz successfully
IF status_a<> 2100 THEN
;add error handling logic here according to different error codes
;e.g.: status=2038 means no point cloud in ROI
halt
ENDIF
;save waypoints of the planned path to jps variables one by one
Xpick_point1=pick_point_a[1]
Xpick_point2=pick_point_a[2]
Xpick_point3=pick_point_a[3]
;follow the planned path to pick
;move to approach waypoint of picking
PTP pick_point1 Vel=50 % PDAT1 Tool[1] Base[0]
;move to picking waypoint
PTP pick_point2 Vel=10 % PDAT2 Tool[1] Base[0]
;add object grasping logic here, such as "$OUT[1]=TRUE"
halt
;move to departure waypoint of picking
PTP pick_point3 Vel=50 % PDAT3 Tool[1] Base[0]
;move to intermediate waypoint of placing
PTP drop_waypoint CONT Vel=100 % PDAT2 Tool[1] Base[0]
;move to approach waypoint of placing
LIN drop_app Vel=1 m/s CPDAT3 Tool[1] Base[0]
;trigger Mech-Viz project and get planned path in advance
$OUT_C[2000]=TRUE
;move to placing waypoint
LIN drop Vel=0.3 m/s CPDAT4 Tool[1] Base[0]
;add object releasing logic here, such as "$OUT[1]=FALSE"
halt
;move to departure waypoint of placing
LIN drop_app Vel=1 m/s CPDAT3 Tool[1] Base[0]
;move back to robot home position
PTP HOME Vel=100 % DEFAULT
ENDLOOP
END위 샘플 프로그램 코드에 해당하는 워크플로는 아래 그림에 표시되어 있습니다.
아래 표는 메인 프로그램 주요 코드의 설명입니다.
| 워크플로 | 코드와 설명 |
|---|---|
Mech-Viz 프로젝트 실행를 트리거하여 경로 계획을 획득하기 |
위 코드는 메인 프로그램에서 $OUT[2000]이 TRUE로 설정되는 것입니다. 이 때 서브 프로그램은 $OUT[2000]이 TRUE로 설정된 경우를 감지하고 Mech-Viz 프로젝트 실행를 트리거하여 경로 계획을 획득합니다. |
루프(피킹→다음 경로 계획을 트리거하기→배치)를 통해 다음 경로를 미리 계획하기 |
위 코드는 메인 프로그램이 LOOP와 ENDLOOP 사이의 코드를 순환 실행함을 나타냅니다. 위 코드는 로봇이 피킹 사전 대기 웨이포인트로 이동한 후, 서브 프로그램이 계획된 경로를 획득하여 저장할 때까지 대기하는 것을 나타냅니다. (즉, $OUT[2000]이 TRUE에서 FALSE로 변경됨). 위 코드는, 서브 프로그램이 가져온 계획 경로(pick_point_a[1] 및 pick_point_a[2], pick_point_a[3])를 각각 Xpick_point1 및 Xpick_point2, Xpick_point3으로 저장하는 것입니다. 위 코드는 로봇이 계획된 경로에 따라 먼저 피킹 접근 웨이포인트(pick_point1)로 이동한 후, 피킹 웨이포인트(pick_point2)로 이동하고 피킹 작업(예:$OUT[1]=TRUE)을 실행한 그 다음 순서대로 피킹 출발 웨이포인트(pick_point3), 배치 중간 웨이포인트(drop_waypoint), 배치 접근 웨이포인트(drop_app)로 이동합니다. 해당 코드는 메인 프로그램이 $OUT_C[2000]을 다시 TRUE로 설정하여 Mech-Viz 프로젝트 실행을 다시 트리거하고 계획된 경로를 가져오는 것을 나타냅니다. 여기는 로봇이 이미 배치 영역에 위치해 있으므로, 배치 완성을 기다릴 필요 없고, 미리 다음 피킹 경로를 계획할 수 있습니다. 위 코드는 로봇이 먼저 배치 포인트(drop)로 이동한 후, 배치 작업(예: $OUT[1]=FALSE)을 수행하고, 그 후 배치 출발 웨이포인트(drop_app)과 Home 포인트로 순차적으로 이동하는 것을 나타냅니다. |