샘플 프로그램7: MM_S7_Viz_SwitchTCP

DEF  MM_S7_Viz_SwitchTCP ( )
;---------------------------------------------------
; FUNCTION: trigger Mech-Viz project and get
; planned path, switch TCP according to the label
; 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
   ;initialize communication parameters (initialization is required only once)
   MM_Init_Socket("XML_Kuka_MMIND",873,871,60)
   ;move to image-capturing position
LIN camera_capture Vel=1 m/s CPDAT1 Tool[1] Base[0]
   ;trigger Mech-Viz project
   MM_Start_Viz(2,init_jps)
   ;get planned path, 1st argument (1) means getting pose in JPs
   MM_Get_VizData(2,pos_num,vis_pos_num,status)
   ;check whether planned path has been got from Mech-Viz successfully
   IF status<> 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 local variables one by one
   MM_Get_Pose(1,Xpick_point1,label[1],toolid[1])
   MM_Get_Pose(2,Xpick_point2,label[2],toolid[2])
   MM_Get_Pose(3,Xpick_point3,label[3],toolid[3])
   ;switch TCP according to the received toolid
   ;reset tool signals according to received toolid
   switch toolid[vis_pos_num]
      CASE 1
         TOOL_DATA[1]=tool_1
         ;reset tool signal
      CASE 2
         TOOL_DATA[1]=tool_2
         ;reset tool signal
      DEFAULT
         ;add handling logic here if the toolid is invalid
         ;reset tool signal
         halt
   endswitch
   ;follow the planned path to pick
   ;move to approach waypoint of picking
LIN pick_point1 Vel=1 m/s CPDAT7 Tool[1] Base[0]
   ;move to picking waypoint
LIN pick_point2 Vel=0.3 m/s CPDAT8 Tool[1] Base[0]
   ;add object grasping logic here, such as "$OUT[1]=TRUE"
   switch toolid[vis_pos_num]
      CASE 1
         halt
         ;open tool signal
      CASE 2
         halt
         ;open tool signal
      DEFAULT
         halt
   endswitch
   ;move to departure waypoint of picking
LIN pick_point3 Vel=1 m/s CPDAT9 Tool[1] Base[0]
   ;move to intermediate waypoint of placing
PTP drop_waypoint CONT Vel=100 % PDAT2 Tool[0] Base[0]
   ;move to approach waypoint of placing
LIN drop_app Vel=1 m/s CPDAT3 Tool[0] Base[0]
   ;move to placing waypoint
LIN drop Vel=0.3 m/s CPDAT4 Tool[0] Base[0]
   ;add object releasing logic here, such as "$OUT[1]=FALSE"
   switch toolid[vis_pos_num]
      CASE 1
         halt
         ;reset tool signal
      CASE 2
         halt
         ;reset tool signal
      DEFAULT
         halt
   endswitch
   ;move to departure waypoint of placing
LIN drop_app Vel=1 m/s CPDAT3 Tool[0] Base[0]
   ;move back to robot home position
PTP HOME Vel=100 % DEFAULT
END

Copied!