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

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

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

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

.. code-block::

    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  

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

.. code-block::

    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    



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

.. code-block:: 

    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

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

.. code-block::

    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