Programme d’exemple 19: MM_S19_Vis_PlanAllVision

DEF  MM_S19_Vis_PlanAllVision ( )
;---------------------------------------------------
; FUNCTION: trigger Mech-Vision project, plan all
; vision results and get all planned paths
; 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)
RECAP:
   ;move to image-capturing position
LIN camera_capture Vel=1 m/s CPDAT1 Tool[1] Base[0]
   ;trigger Mech-Viz project
   MM_Start_Vis(1,0,2,init_jps)
   ;get planned path from NO.1 Mech-Vision project; 2nd argument (1) means getting pose in JPs
   MM_Get_Vispath(1,1,pos_num,vis_pos_num,status)
   ;check whether planned path has been got from Mech-Vision successfully
   IF status<> 1103 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 results
      halt
   ENDIF
   ;save waypoints of the planned path to local variables one by one
   FOR count=1 TO pos_num
      MM_Get_Jps(count,pick_point[count],label[count],toolid[count])
   ENDFOR
   ;parse pick cycle count, here suppose 5 points per planned path
   pick_cnt = pos_num / 5
   residual = pos_num - pick_cnt*5
   ;check if parsed data is valid; if not, retry to get planned path or add some error handling logic
   IF (pick_cnt<1) OR (residual<>0) THEN
      halt
      GOTO RECAP
   ENDIF
   ;repeatedly run pick-and-place cycle using for-loop
   FOR i=1 TO pick_cnt
      count=(i-1)*5
      Xpick_point1=pick_point[1+count]
      Xpick_point2=pick_point[2+count]
      Xpick_point3=pick_point[3+count]
      Xpick_point4=pick_point[4+count]
      Xpick_point5=pick_point[5+count]
      ;follow the planned path to pick
      ;move to approach waypoint of picking
PTP pick_point1 CONT Vel=50 % PDAT1 Tool[1] Base[0]
PTP pick_point2 Vel=50 % PDAT2 Tool[1] Base[0]
PTP pick_point3 Vel=10 % PDAT3 Tool[1] Base[0]
      ;add object grasping logic here, such as "$OUT[1]=TRUE"
      halt
PTP pick_point4 Vel=50 % PDAT5 Tool[1] Base[0]
PTP pick_point5 CONT Vel=50 % PDAT6 Tool[1] Base[0]
      ;move to intermediate waypoint of placing
PTP drop_waypoint CONT Vel=100 % PDAT4 Tool[1] Base[0]
      ;move to approach waypoint of placing
LIN drop_app Vel=1 m/s CPDAT2 Tool[1] Base[0]
      ;move to placing waypoint
LIN drop Vel=0.3 m/s CPDAT3 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 CPDAT2 Tool[1] Base[0]
      ;move to intermediate waypoint of placing
PTP drop_waypoint CONT Vel=100 % PDAT4 Tool[1] Base[0]
   ENDFOR
   ;finish pick and-place cycle, and jump back to camera capturing
   GOTO RECAP
END