案例/模板说明

机械臂主控主函数说明

注意对工控机返回的 err_code 进行判断。

座标移动基础模板

NOP
PRINTMSG 0 #S1,S2: initialization and connect to IPC# DISABLE 初始化并连接工控机
CLOSEMSG ID = 1
OPENMSG ID = 1
SET B001 = 1
TIMER T = 1
MSG_CONN_ST 1 B001
IF {(B001 != 1)}
PRINTMSG 2 #socket connection failed!# 套接字连接失败
STOPRUN
ENDIF
SET GS002 = #cart_basic.t# DISABLE 切换任务
CALL [$xyzSwitchTask$] DISABLE
IF {(GD500 != 0)} DISABLE
STOPRUN DISABLE
ENDIF DISABLE
PRINTMSG 0 #S3-------------# DISABLE 切换工件
SET GI001 = 0
SET GS006 = #item1#
CALL [$xyzSwitchItem$]
IF {(GD500 != 0)}
STOPRUN
ENDIF
PRINTMSG 0 #S4: move to home# DISABLE 移动到初始位姿
DOUT OT#(1) 1 T = 0 0
POSSETALL P0001 RF  0 0 0 0 0 0 0 0 0 0 0 0 0 0
MOVJ P0001 VJ = 10 % PL = 0 ACC = 10 DEC = 10 0
PRINTMSG 0 #S5: if eye in hand or not# DISABLE 是否眼在手上
SET B001 = 1
LABEL [$loop$]
IF {(B001 == 1)}
PRINTMSG 0 #S6: move to scan pose# DISABLE 移动到拍照位姿
POSSETALL P0002 RF  0 0 0 0 0 0 0 0 0 0 0 0 0 0
MOVL P0002 V = 10 mm/s PL = 0 ACC = 1 DEC = 1 0
PRINTMSG 0 #S7: send scan pose# DISABLE 发送拍照位姿
CALL [$xyzSendCurCart$]
IF {(GD500 != 0)}
STOPRUN
ENDIF
ENDIF
PRINTMSG 0 #S8: request grasp pose# DISABLE 请求抓取位姿
SET GI003 = 0
CALL [$xyzReqGraspPose$]
PRINTMSG 0 #S9: get grasp pose# DISABLE 获取抓取位姿
CALL [$xyzGetGraspPose$]
POSSETALL GP0001 BF  GD501 GD502 GD503 GD504 GD505 GD506 0 0 0 0 0 0 0 0
IF {(GD508 < 1)}
PRINTMSG 1 #No grasp pose, continue requesting# 没有抓取位姿，继续请求
JUMP [$loop$]
ENDIF
PRINTMSG 0 #S10: move to grasp pose# DISABLE 移动到抓取位姿
PRINTMSG 0 #Attention: Additional waypoints maybe need added!# DISABLE 注意：可能需要添加额外的路径点
MOVL GP0001 V = 10 mm/s PL = 0 ACC = 1 DEC = 1 0
DOUT OT#(1) 0 T = 0 0
PRINTMSG 0 #S11: move to place pose# DISABLE
PRINTMSG 0 #Attention: Additional waypoints maybe need added!# DISABLE 注意：可能需要添加额外的路径点
POSSETALL P0003 RF  0 0 0 0 0 0 0 0 0 0 0 0 0 0
MOVL P0003 V = 10 mm/s PL = 0 ACC = 1 DEC = 1 0
DOUT OT#(1) 1 T = 0 0
JUMP [$loop$]
END

座标移动二次定位模板：

NOP
PRINTMSG 0 #S1,S2: initialization and connect to IPC# DISABLE 初始化并连接工控机
CLOSEMSG ID = 1
OPENMSG ID = 1
SET B001 = 1
TIMER T = 1
MSG_CONN_ST 1 B001
IF {(B001 != 1)}
PRINTMSG 2 #socket connection failed!# 套接字连接失败
STOPRUN
ENDIF
SET GS002 = #cart_repo.t# DISABLE 切换任务
CALL [$xyzSwitchTask$] DISABLE
IF {(GD500 != 0)} DISABLE
STOPRUN DISABLE
ENDIF DISABLE
PRINTMSG 0 #S3-------------# DISABLE 切换工件
SET GI001 = 0
SET GS005 = #item1#
CALL [$xyzSwitchItem$]
IF {(GD500 != 0)}
STOPRUN
ENDIF
PRINTMSG 0 #S4: move to home# DISABLE 移动到初始位姿
POSSETALL P0001 RF  0 0 0 0 0 0 0 0 0 0 0 0 0 0
MOVJ P0001 VJ = 10 % PL = 0 ACC = 10 DEC = 10 0
DOUT OT#(1) 0 T = 0 0
DOUT OT#(2) 0 T = 0 0
LABEL [$loop$]
PRINTMSG 0 #S5: req grasp pose# DISABLE 请求抓取位姿
SET GI001 = 0
CALL [$xyzReqGraspPose$]
IF {(GD500 != 0)}
STOPRUN
ENDIF
PRINTMSG 0 #S6: get grasp pose# DISABLE 获取抓取位姿
CALL [$xyzGetGraspPose$]
POSSETALL GP0001 BF  GD501 GD502 GD503 GD504 GD505 GD506 0 0 0 0 0 0 0 0
IF {(GD500 != 0)}
STOPRUN
ENDIF
IF {(GD508 < 1)}
PRINTMSG 0 #S15: No grasp pose, grasp board# DISABLE 没有抓取位姿，抓取隔板
SET GI001 = 0
SET GS005 = #board#
CALL [$xyzSwitchItem$]
IF {(GD500 != 0)}
STOPRUN
ENDIF
PRINTMSG 0 #S16: req board grasp pose# DISABLE 请求隔板抓取位姿
SET GI001 = 0
CALL [$xyzReqGraspPose$]
PRINTMSG 0 #S17: get board grasp pose# DISABLE 获取隔板抓取位姿
CALL [$xyzGetGraspPose$]
IF {(GD500 != 0)}
STOPRUN
ENDIF
POSSETALL GP0001 BF GD501 GD502 GD503 GD504 GD505 GD506 0 0 0 0 0 0 0 0
IF {(GD508 < 1)}
PRINTMSG 1 #No board, you can change grasp tote# 没有隔板，可以换筐
STOPRUN
ENDIF
PRINTMSG 0 #S18: move to board grasp pose# DISABLE 移动到隔板抓取位姿
PRINTMSG 0 #Attention: Additional waypoints maybe need added!# DISABLE 注意：可能需要添加额外的路径点
POSSETALL GP0001 BF GD501 GD502 GD503 GD504 GD505 GD506 0 0 0 0 0 0 0 0
MOVL GP0001 V = 10 mm/s PL = 0 ACC = 1 DEC = 1 0
DOUT OT#(1) 1 T = 0 0
PRINTMSG 0 #S19: move to board grasp pose# DISABLE 移动到隔板放置位姿
PRINTMSG 0 #Attention: Additional waypoints maybe need added!# DISABLE 注意：可能需要添加额外的路径点
POSSETALL GP0001 BF  111 222 333 1.5708 0 0 0 0 0 0 0 0 0 0
MOVL GP0001 V = 10 mm/s PL = 0 ACC = 1 DEC = 1 0
DOUT OT#(1) 0 T = 0 0
PRINTMSG 0 #S20: switch item# DISABLE 切换抓取物体为工件
SET GI001 = 0
SET GS005 = #item1#
CALL [$xyzSwitchItem$]
IF {(GD500 != 0)}
STOPRUN
ENDIF
JUMP [$loop$]
ENDIF
PRINTMSG 0 #S8: move to scan pose# DISABLE 移动到拍照位姿
MOVL GP0001 V = 10 mm/s PL = 0 ACC = 1 DEC = 1 0
PRINTMSG 0 #S9: send scan pose# DISABLE 发送拍照位姿
CALL [$xyzSendCurCart$]
IF {(GD500 != 0)}
STOPRUN
ENDIF
PRINTMSG 0 #S10: switch item# DISABLE 切换抓取物体为工件
SET GI001 = 1
SET GS005 = #item1#
CALL [$xyzSwitchItem$]
IF {(GD500 != 0)}
STOPRUN
ENDIF
PRINTMSG 0 #S11: req grasp pose# DISABLE 请求抓取位姿
SET GI001 = 1
CALL [$xyzReqGraspPose$]
PRINTMSG 0 #S12: get grasp pose# DISABLE 获取抓取位姿
CALL [$xyzGetGraspPose$]
IF {(GD500 != 0)}
STOPRUN
ENDIF
PRINTMSG 0 #S13: move to grasp pose# DISABLE 移动到抓取位姿
PRINTMSG 0 #Attention: Additional waypoints maybe need added!# DISABLE 注意：可能需要添加额外的路径点
POSSETALL GP0001 BF  GD501 GD502 GD503 GD504 GD505 GD506 0 0 0 0 0 0 0 0
IF {(GD508 < 1)}
PRINTMSG 1 #No fine grasp pose, stop# 没有精确抓取位姿，停止
STOPRUN
ENDIF
MOVL GP0001 V = 10 mm/s PL = 0 ACC = 1 DEC = 1 0
DOUT OT#(2) 1 T = 0 0
PRINTMSG 0 #S14: move to place pose# DISABLE 移动到放置位姿
PRINTMSG 0 #Attention: Additional waypoints maybe need added!# DISABLE 注意：可能需要添加额外的路径点
POSSETALL P0002 RF  0 0 0 0 0 0 0 0 0 0 0 0 0 0
MOVL P0002 V = 10 mm/s PL = 0 ACC = 1 DEC = 1 0
DOUT OT#(2) 0 T = 0 0
JUMP [$loop$]
END

轨迹移动同步模板：

NOP
PRINTMSG 0 #S1,S2: initialization and connect to IPC# DISABLE 初始化并连接工控机
CLOSEMSG ID = 1
OPENMSG ID = 1
SET B001 = 1
TIMER T = 1
MSG_CONN_ST 1 B001
IF {(B001 != 1)}
PRINTMSG 2 #socket connection failed!# 套接字连接失败
STOPRUN
ENDIF
SET GS002 = #traj_sync.t# DISABLE 切换任务
CALL [$xyzSwitchTask$] DISABLE
IF {(GD500 != 0)} DISABLE
STOPRUN DISABLE
ENDIF DISABLE
PRINTMSG 0 #S3: switch item# DISABLE 切换工件
SET GI001 = 0
SET GS005 = #item1#
CALL [$xyzSwitchItem$]
IF {(GD500 != 0)}
STOPRUN
ENDIF
PRINTMSG 0 #S4: move to home#  DISABLE 移动到初始位姿
DOUT OT#(1) 1 T = 0 0
MOVJ P0002 VJ = 10 % PL = 0 ACC = 10 DEC = 10 0
LABEL [$loop$]
PRINTMSG 0 #S5: req pick and place# DISABLE 请求计算抓取放置轨迹
SET GI001 = 0
CALL [$xyzReqPickPlace$]
IF {(GD500 != 0)}
STOPRUN
ENDIF
PRINTMSG 0 #S6: get pick in trajectory# DISABLE 获取抓取入筐轨迹
SET GI001 = 0
CALL [$xyzGetPickIn$]
IF {(GD500 != 0)}
PRINTMSG 0 #S15: no item found#
STOPRUN
ENDIF
PRINTMSG 0 #S8: execute pick in trajectory#  DISABLE 执行抓取入筐轨迹
CALL [$xyzExePickinTraj$]
PRINTMSG 0 #S9: get pick out trajectory# DISABLE 获取抓取出筐轨迹
SET GI001 = 0
CALL [$xyzGetPickOut$]
IF {(GD500 != 0)}
STOPRUN
ENDIF
PRINTMSG 0 #S10: execute pick out trajectory# DISABLE 执行抓取出筐轨迹
CALL [$xyzExePikoutTraj$]
PRINTMSG 0 #S11: get place in trajectory# DISABLE 获取放置入筐轨迹
SET GI001 = 0
CALL [$xyzGetPlaceIn$]
IF {(GD500 != 0)}
STOPRUN
ENDIF
PRINTMSG 0 #S12: execute place in trajectory# DISABLE 执行放置入筐轨迹
CALL [$xyzExeTraj$]
DOUT OT#(1) 0 T = 0 0
PRINTMSG 0 #S13: get place out trajectory# DISABLE 获取放置出筐轨迹
SET GI001 = 0
CALL [$xyzGetPlaceOut$]
IF {(GD500 != 0)}
STOPRUN
ENDIF
PRINTMSG 0 #S14: execute place out trajectory# DISABLE 执行放置出筐轨迹
CALL [$xyzExeTraj$]
JUMP [$loop$]
END

轨迹移动异步模板

NOP
PRINTMSG 0 #S1,S2: initialization and connect to IPC# DISABLE 初始化并连接工控机
CLOSEMSG ID = 1
OPENMSG ID = 1
SET B001 = 1
TIMER T = 1
MSG_CONN_ST 1 B001
IF {(B001 != 1)}
PRINTMSG 2 #socket connection failed!# 套接字连接失败
STOPRUN
ENDIF
SET GS002 = #traj_async.t# DISABLE 切换任务
CALL [$xyzSwitchFlow$] DISABLE
IF {(GD500 != 0)} DISABLE
STOPRUN DISABLE
ENDIF DISABLE
PRINTMSG 0 #S3: switch item# DISABLE 切换工件
SET GI001 = 0
SET GS005 = #item1#
CALL [$xyzSwitchItem$]
IF {(GD500 != 0)}
STOPRUN
ENDIF
PRINTMSG 0 #S4: move to home# DISABLE 移动到初始位姿
MOVJ P0001 VJ = 10 % PL = 0 ACC = 10 DEC = 10 0
DOUT OT#(1) 1 T = 0 0
LABEL [$loop$]
PRINTMSG 0 #S5: req pick and place# DISABLE 请求计算抓取放置轨迹
SET GI001 = 0
CALL [$xyzReqPickPlace$]
IF {(GD500 != 0)}
STOPRUN
ENDIF
PRINTMSG 0 #S6: get pick in trajectory# DISABLE 获取抓取入筐轨迹
SET GI001 = 0
CALL [$xyzGetPickIn$]
IF {(GD500 != 0)}
STOPRUN
ENDIF
IF {(GD503 < 1)}
PRINTMSG 0 #S16: no item found# DISABLE 没有找到物体
STOPRUN
ENDIF
PRINTMSG 0 #S8: execute pick in trajectory# DISABLE 执行抓取入筐轨迹
CALL [$xyzExePickinTraj$]
PRINTMSG 0 #S9: get pick out trajectory# DISABLE 获取抓取出筐轨迹
SET GI001 = 0
CALL [$xyzGetPickOut$]
IF {(GD500 != 0)}
STOPRUN
ENDIF
PRINTMSG 0 #S10: execute pick out trajectory# DISABLE 执行抓取出筐轨迹
CALL [$xyzExePikoutTraj$]
PRINTMSG 0 #S11: req pick and place in advance# DISABLE 请求提前计算抓取放置轨迹
SET GI001 = 0
CALL [$xyzReqPickPlace$]
IF {(GD500 != 0)}
STOPRUN
SET GI001 = 0
PRINTMSG 0 #S12: get place in trajectory# DISABLE 获取放置入筐轨迹
CALL [$xyzGetPlaceIn$]
IF {(GD500 != 0)}
STOPRUN
ENDIF
PRINTMSG 0 #S13: execute place in trajectory# DISABLE 执行放置入筐轨迹
CALL [$xyzExeTraj$]
DOUT OT#(1) 0 T = 0 0
PRINTMSG 0 #S14: get place out trajectory# DISABLE 获取放置出筐轨迹
SET GI001 = 0
CALL [$xyzGetPlaceOut$]
IF {(GD500 != 0)}
STOPRUN
ENDIF
PRINTMSG 0 #S15: execute place out trajectory# DISABLE 执行放置出筐轨迹
CALL [$xyzExeTraj$]
JUMP [$loop$]
END