样例程序6：MM_S6_Viz_ErrorHandle
程序简介
功能说明 |
机器人触发Mech-Viz工程运行，并获取路径规划结果，然后根据状态码判断结果是否成功获取到规划路径。如果成功获取规划路径，则机器人执行抓取和放置操作；否则，机器人暂停运行。 |
文件路径 |
Mech-Vision和Mech-Viz软件安装目录下 |
所需工程 |
Mech-Vision工程和Mech-Viz工程 |
使用前提 |
| 此样例程序仅是示例程序。用户需根据实际情况在此基础上进行修改，请勿直接使用该程序。 |
程序解读
以下为MM_S6_Viz_ErrorHandle样例程序的代码及相关解释说明。
| 与MM_S2_Viz_Basic样例相比，本样例仅新增了对不同错误状态码进行处理的功能（加粗部分的代码）。因此，下文不再重复解释与MM_S2_Viz_Basic样例相同部分的代码（详情请参考MM_S2_Viz_Basic样例说明）。 |
NOP
'--------------------------------
'FUNCTION: trigger Mech-Viz
'project and get planned path,
'handle errors according to
'status codes (if no point cloud
'in ROI, retry several times
'before exit loop)
'Mech-Mind, 2023-12-25
'--------------------------------
'clear I50 to I69
CLEAR I050 20
'initialize p variables
SUB P071 P071
SUB P072 P072
SUB P073 P073
'initialize i variables
SET I005 0
'move to robot home position
MOVJ C00000 VJ=50.00
'initialize communication
'parameters (initialization is
'required only once)
CALL JOB:MM_INIT_SOCKET ARGF"192.168.170.22;50000;1"
'move to image-capturing position
MOVJ C00001 VJ=50.00 PL=0
'open socket connection
CALL JOB:MM_OPEN_SOCKET
*RECAP
'trigger Mech-Viz project
CALL JOB:MM_START_VIZ ARGF"2;30"
'get planned path, 1st argument
'(1) means getting pose in JPs
CALL JOB:MM_GET_VIZDATA ARGF"1;51;52;53"
'check whether planned path has
'been got from Mech-Viz
'successfully
IFTHENEXP I053<>2100
IFTHENEXP I053=2038
'no point cloud in ROI, add
'error handling logic here
'self-adding then check retry
'counter
INC I005
IFTHENEXP I005<3
'jump back to vision retry label
'if the number of retry times is
'less than 3
JUMP *RECAP
ELSE
'reset counter and exit loop if
'the number of retry times has
'reached 3
SET I005 0
JUMP *ENDLOOP
ENDIF
ELSE
'add other error handling logic
'here
PAUSE
JUMP *ENDLOOP
ENDIF
ENDIF
'close socket connection
CALL JOB:MM_CLOSE_SOCKET
'save waypoints of the planned
'path to local variables one
'by one
CALL JOB:MM_GET_JPS ARGF"1;71;61;62"
CALL JOB:MM_GET_JPS ARGF"2;72;63;64"
CALL JOB:MM_GET_JPS ARGF"3;73;65;66"
'follow the planned path to pick
'move to approach waypoint of
'picking
MOVJ P071 VJ=50.00 PL=0
'move to picking waypoint
MOVJ P072 VJ=10.00 PL=0
'add object grasping logic here,
'such as DOUT OT#(1) ON
PAUSE
'move to departure waypoint of
'picking
MOVJ P073 VJ=50.00 PL=0
'move to intermediate waypoint of
'placing
MOVJ C00002 VJ=50.00
'move to approach waypoint of
'placing
MOVL C00003 V=166.6 PL=0
'move to placing waypoint
MOVL C00004 V=50.0 PL=0
'add object releasing logic here,
'such as DOUT OT#(1) OFF
PAUSE
'move to departure waypoint of
'placing
MOVL C00005 V=166.6 PL=0
'move back to robot home position
MOVJ C00006 VJ=50.00
*ENDLOOP
PAUSE
END上述样例程序代码对应的流程如下图所示。
下表为处理不同状态码的逻辑解读。
| 流程 | 代码及说明 | ||
|---|---|---|---|
处理不同状态码 |
机器人在执行MM_GET_VIZDATA指令后，会将指令执行的状态码保存至I053变量。用户可根据具体状态码做相应的处理，如本例中的处理逻辑。
|