样例程序13：MM_S13_Vis_MoveInAdvance

DEF  MM_S13_Vis_MoveInAdvance ( )
;---------------------------------------------------
; FUNCTION: trigger Mech-Vision project then move
; to wait position and get vision pose in EIH setup
; 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 NO.1 Mech-Vision project
   MM_Start_Vis(1,0,1,init_jps)
   ;move to wait position for picking
LIN pick_wait_point CONT Vel=1 m/s CPDAT6 Tool[1] Base[0]
   ;get vision result from NO.1 Mech-Vision project
   MM_Get_VisData(1,pos_num,status)
   ;check whether vision result has been got from Mech-Vision successfully
   IF status<> 1100 THEN
      ;add error handling logic here according to different error codes
      ;e.g.: status=1003 means no point cloud in ROI
      ;e.g.: status=1002 means no vision result
      halt
   ENDIF
   ;save first vision point data to local variables
   MM_Get_Pose(1,Xpick_point,label,toolid)
   ;calculate pick approach point based on pick point
   tool_offset={X 0,Y 0,Z -100,A 0,B 0,C 0}
   Xpick_app=Xpick_point:tool_offset
   ;move to intermediate waypoint of picking
PTP pick_waypoint CONT Vel=50 % PDAT1 Tool[1] Base[0]
   ;move to approach waypoint of picking
LIN pick_app Vel=1 m/s CPDAT2 Tool[1] Base[0]
   ;move to picking waypoint
LIN pick_point Vel=0.3 m/s CPDAT3 Tool[1] Base[0]
   ;add object grasping logic here, such as "$OUT[1]=TRUE"
   halt
   ;move to departure waypoint of picking
LIN pick_app Vel=1 m/s CPDAT2 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 CPDAT4 Tool[1] Base[0]
   ;move to placing waypoint
LIN drop Vel=0.3 m/s CPDAT5 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 CPDAT4 Tool[1] Base[0]
   ;move back to robot home position
PTP HOME Vel=100 % DEFAULT
END

Copied!