案例/模板说明
============================

机械臂主控主函数说明
++++++++++++++++++++++++++++++++

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

轨迹移动同步模板：
----------------------------------

.. code-block:: 

    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

轨迹移动异步模板
-------------------------------------

.. code-block::

    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

座标移动基础模板：
-------------------------------

.. code-block::

    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    


座标移动二次定位模板
--------------------------------------

.. code-block::

    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