样例程序7：MM_S7_Viz_SwitchTCP
程序解读
以下为MM_S7_Viz_SwitchTCP样例程序的代码及相关解释说明。
| 与MM_S2_Viz_Basic样例相比，本样例主要新增了根据末端工具编号切换工具的功能（加粗部分的代码）。因此，下文不再重复解释与MM_S2_Viz_Basic样例相同部分的代码（详情请参考MM_S2_Viz_Basic样例说明）。 |
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上述样例程序代码对应的流程如下图所示。
下表为新增代码的逻辑解读。
| 流程 | 代码及说明 | ||
|---|---|---|---|
获取规划路径 |
因此，整条指令表示机器人获取Mech-Viz工程返回的规划路径。
|
||
转存规划路径 |
因此，“MM_Get_Pose(1,Xpick_point1,label[1],toolid[1])”整条指令表示将第一个路径点的工具位姿、标签和末端工具编号分别转存至指定变量。
|
||
根据末端工具编号切换末端工具 |
toolid[vis_pos_num]表示抓取点的末端工具编号值。用户可根据toolid[vis_pos_num]变量值，决定机器人移动到对应位姿（抓取点）时所使用的末端工具，即根据末端工具编号切换末端工具。上述代码的逻辑说明如下所示。
当机器人移动至抓取点时，用户可根据末端工具编号打开相应工具对应的DO信号，例如样例中如下代码。 当机器人移动至放置点时，用户可根据末端工具编号关闭并重置相应工具对应的DO信号，例如样例中如下代码。 |