案例/模板说明

机械臂主控主函数说明

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

轨迹移动同步模板：

NOP
CLOSEMSG ID = 1 关闭socket连接
OPENMSG ID = 1 打开socket连接
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# 切换任务
CALL [$xyzSwitchFlow$]
IF {(GD500 != 0)}
STOPRUN
ENDIF
SET GI003 = 0
SET GS005 = #item1# 切换工件
CALL [$xyzSwitchItem$]
IF {(GD500 != 0)}
STOPRUN
ENDIF
DOUT OT#(1) 1 T = 0 0  初始化 IO
MOVJ P0002 VJ = 10 % PL = 0 ACC = 10 DEC = 10 0 移动到初始位姿
LABEL [$loop$]
SET GI003 = 0
CALL [$xyzReqPickPlace$] 请求计算抓取放置轨迹
IF {(GD500 != 0)}
STOPRUN
ENDIF
SET GI003 = 0
CALL [$xyzGetPickIn$] 获取抓取入筐轨迹
IF {(GD500 != 0)}
STOPRUN
ENDIF
CALL [$xyzExePickinTraj$] 执行抓取入筐轨迹
SET GI003 = 0
CALL [$xyzGetPickOut$] 获取抓取出筐轨迹
IF {(GD500 != 0)}
STOPRUN
ENDIF
CALL [$xyzExePikoutTraj$] 执行抓取出筐轨迹
SET GI003 = 0
CALL [$xyzGetPlaceIn$] 获取放置入筐轨迹
IF {(GD500 != 0)}
STOPRUN
ENDIF
DOUT OT#(1) 0 T = 0 0
CALL [$xyzExeTraj$] 执行放置入筐轨迹
SET GI003 = 0
CALL [$xyzGetPlaceOut$] 获取放置出筐轨迹
IF {(GD500 != 0)}
STOPRUN
ENDIF
CALL [$xyzExeTraj$] 执行放置出筐轨迹
JUMP [$loop$]
END

轨迹移动异步模板

NOP
CLOSEMSG ID = 1 关闭socket连接
OPENMSG ID = 1  打开socket连接
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# 切换任务
CALL [$xyzSwitchFlow$]
IF {(GD500 != 0)}
STOPRUN
ENDIF
SET GI003 = 0
SET GS005 = #item1# 切换工件
CALL [$xyzSwitchItem$]
IF {(GD500 != 0)}
STOPRUN
ENDIF
DOUT OT#(1) 1 T = 0 0
MOVJ P0001 VJ = 10 % PL = 0 ACC = 10 DEC = 10 0
SET GI003 = 0
CALL [$xyzReqPickPlace$] 请求计算抓取放置轨迹
IF {(GD500 != 0)}
STOPRUN
ENDIF
LABEL [$loop$]
SET GI003 = 0
CALL [$xyzGetPickIn$] 获取抓取入筐轨迹
IF {(GD500 != 0)}
STOPRUN
ENDIF
IF {(GD503 < 1)}
STOPRUN
ENDIF
CALL [$xyzExePickinTraj$]  执行抓取入筐轨迹
SET GI003 = 0
CALL [$xyzGetPickOut$] 获取抓取出筐轨迹
IF {(GD500 != 0)}
STOPRUN
ENDIF
CALL [$xyzExePikoutTraj$] 执行抓取出筐轨迹
SET GI003 = 0
CALL [$xyzReqPickPlace$] 提前请求计算抓取放置轨迹
IF {(GD500 != 0)}
STOPRUN
SET GI003 = 0
CALL [$xyzGetPlaceIn$] 获取放置入筐轨迹
IF {(GD500 != 0)}
STOPRUN
ENDIF
DOUT OT#(1) 0 T = 0 0
CALL [$xyzExeTraj$] 执行放置入筐轨迹
SET GI003 = 0
CALL [$xyzGetPlaceOut$] 获取放置出筐轨迹
IF {(GD500 != 0)}
STOPRUN
ENDIF
CALL [$xyzExeTraj$] 执行放置出筐轨迹
JUMP [$loop$]
END

座标移动基础模板：

NOP
CLOSEMSG ID = 1 关闭socket连接
OPENMSG ID = 1 打开socket连接
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# 切换任务
CALL [$xyzSwitchFlow$]
IF {(GD500 != 0)}
STOPRUN
ENDIF
SET GI003 = 0
SET GS005 = #item1# 切换工件
CALL [$xyzSwitchItem$]
IF {(GD500 != 0)}
STOPRUN
ENDIF
DOUT OT#(1) 1 T = 0 0 初始化IO
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 移动到初始位姿
SET B001 = 1
LABEL [$loop$]
IF {(B001 == 1)}
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 移动到拍照位姿
CALL [$xyzSendCurCart$] 发送当前位姿
IF {(GD500 != 0)}
STOPRUN
ENDIF
ENDIF
SET GI003 = 0
CALL [$xyzReqGraspPose$] 请求抓取位姿
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
MOVL GP0001 V = 10 mm/s PL = 0 ACC = 1 DEC = 1 0 移动到抓取位姿
DOUT OT#(1) 0 T = 0 0 抓取
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
CLOSEMSG ID = 1 关闭socket连接
OPENMSG ID = 1 打开socket连接
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# 切换任务
CALL [$xyzSwitchFlow$]
IF {(GD500 != 0)}
STOPRUN
ENDIF
DOUT OT#(1) 0 T = 0 0 初始化IO
DOUT OT#(2) 0 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 移动到初始位姿
SET GI003 = 0
SET GS005 = #item1# 切换工件
CALL [$xyzSwitchItem$]
IF {(GD500 != 0)}
STOPRUN
ENDIF
SET GI003 = 0
CALL [$xyzReqGraspPose$] 请求粗抓取位姿
IF {(GD500 != 0)}
STOPRUN
ENDIF
LABEL [$loop$]
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)} 如果没有抓取到工件，尝试抓隔板
SET GI003 = 0
SET GS005 = #board# 切换抓取物体为隔板
CALL [$xyzSwitchItem$]
IF {(GD500 != 0)}
STOPRUN
ENDIF
SET GI003 = 0
CALL [$xyzReqGraspPose$] 请求隔板抓取位姿
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
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 抓取
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 放置
SET GI003 = 0
SET GS005 = #item1# 切换抓取物体为工件
CALL [$xyzSwitchItem$]
IF {(GD500 != 0)}
STOPRUN
ENDIF
SET GI003 = 0
CALL [$xyzReqGraspPose$] 请求粗抓取位姿
IF {(GD500 != 0)}
STOPRUN
ENDIF
JUMP [$loop$]
ENDIF
MOVL GP0001 V = 10 mm/s PL = 0 ACC = 1 DEC = 1 0 移动到粗抓取位姿
CALL [$xyzSendCurCart$] 发送当前位姿
IF {(GD500 != 0)}
STOPRUN
ENDIF
SET GI003 = 1
SET GS005 = #item1# 切换抓取物体为工件
CALL [$xyzSwitchItem$]
IF {(GD500 != 0)}
STOPRUN
ENDIF
SET GI003 = 1
CALL [$xyzReqGraspPose$] 请求精确抓取位姿
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 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 抓取
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 放置
SET GI003 = 0
SET GS005 = #item1# 切换抓取物体为工件
CALL [$xyzSwitchItem$]
IF {(GD500 != 0)}
STOPRUN
ENDIF
SET GI003 = 0
CALL [$xyzReqGraspPose$] 请求粗抓取位姿
IF {(GD500 != 0)}
STOPRUN
ENDIF
JUMP [$loop$]
END