妗堜緥/妯℃澘璇存槑
============================

鏈烘鑷備富鎺х▼搴忚鏄�
++++++++++++++++++++++++++++++++
浠ヤ笅涓烘満姊拌噦涓绘帶妯℃澘浠ｇ爜锛屾敞鎰忓宸ユ帶鏈鸿繑鍥炵殑 err_code 杩涜鍒ゆ柇銆�


搴ф爣绉诲姩鍩虹妯℃澘
+++++++++++++++++++++++++++++++++++++++++

.. code-block::

    DEF xyz_CartMove_template()
        ; S1: Initialization
        ; S1: 鍒濆鍖�
        INT err_code ; 閿欒鐮�
        INT grasp_pose_token ; 鎶撳彇浣嶅Э token
        E6POS grasp_pose ; 鎶撳彇浣嶅Э
        E6POS home_pose ; home 浣嶅Э
        E6POS scan_pose ; 鎷嶇収浣嶅Э
        E6POS place_pose ; 鏀剧疆浣嶅Э
        INT grasp_pose_num ; 鎶撳彇浣嶅Э鏁伴噺
        BOOL is_eye_in_hand ; 鏄惁涓虹溂鍦ㄦ墜涓�
        INT obj_pipeline_num ; 鐗╀綋杩愬姩娴佺▼缂栧彿
        INT obj_register_num 锛涚墿浣撴姄鍙栧簭鍙�
        INT ints[6]
        INT vs_id ; 瑙嗚缂栧彿
        CHAR item_codename[20] ; 鐗╀綋浠ｅ彿
        CHAR task_codename[20] 锛涗换鍔′唬鍙�

        grasp_pose_token = 0
        gMasterFlag = TRUE
        err_code = 0
        grasp_pose_num = 0
        obj_pipeline_num = 0
        obj_register_num = 0

        ; 浠ヤ笅涓轰腑鏂鐞嗙瓑浠ｇ爜锛屼笉鑳藉垹闄�
        GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( )
        INTERRUPT ON 3
        BAS (#INITMOV,0 )
        ; The first movement for Kuka must be a PTP movement
        $BWDSTART = FALSE
        PDAT_ACT = PPDAT_1
        FDAT_ACT = FPDAT_2
        BAS(#PTP_PARAMS, PPDAT_1.vel)
        ; Comment out this line if there is an error
        SET_CD_PARAMS (0)
        PTP $AXIS_ACT

        ; S2: Connect to IPC
        ; S2: 杩炴帴 IPC
        xyzOpenSocket("MasterClient") ; 杩炴帴 IPC

        ; gStrOpSuccess = STRCLEAR(task_codename[]) ; 蹇呴』鍏堟竻绌哄瓧绗︿覆    
        ; task_codename[] = "cart_move.t"  
        ; err_code = xyzSwitchTask(task_codename[]) ; 鍒囨崲浠诲姟
        ; IF (err_code <> 0) THEN  ;
        ;     GOTO END_PROGRAM;
        ; ENDIF

        ; S3: Switch object
        ; S3: 鍒囨崲鐗╀綋
        vs_id = 0
        gStrOpSuccess = STRCLEAR(item_codename[]) ; 蹇呴』鍏堟竻绌哄瓧绗︿覆          
        item_codename[] = "item1"
        err_code = xyzSwitchItem(vs_id, item_codename[]) ; 鍒囨崲宸ヤ欢
        IF (err_code <> 0) THEN  ;
            HALT;
            GOTO END_PROGRAM;
        ENDIF

        ; S4: Move to home pose
        ; S4: 绉诲姩鍒� home 浣嶅Э
        ; you should use WorkVisual to set the home_pose,
        ; or use pendant to set the home_pose
        ; 浣跨敤 WorkVisual 璁剧疆 home 浣嶅Э锛屾垨鑰呬娇鐢ㄧず鏁欏櫒璁剧疆 home 浣嶅Э
        home_pose = {X 578.80, Y -9.12, Z 541.47, A 173.34,B -4.38,C -177.88}
        xyzSetMoveJParameters(10,20,0) ; 璁剧疆鍏宠妭绉诲姩鍙傛暟: 閫熷害銆佸姞閫熷害銆佸噺閫熷害
        PTP home_pose ; 鍏宠妭绉诲姩鍒� home 浣嶅Э
        ; xyzSetMoveLParameters(10,20,0) ; 璁剧疆鐩寸嚎绉诲姩鍙傛暟: 閫熷害銆佸姞閫熷害銆佸噺閫熷害
        ; LIN home_pose ; 鐩寸嚎绉诲姩鍒� home 浣嶅Э
        ; $OUT[500] = TRUE; IO 鎿嶄綔

        ; if application is eye_on_hand, please make eye_on_hand := true;
        ; 濡傛灉鏄溂鍦ㄦ墜涓婄殑搴旂敤锛岃灏� eye_on_hand := true;
        is_eye_in_hand = false;

        BEGIN_WHILE:
        WHILE TRUE
            ; S5: eye on hand application
            ; S5: 鐪煎湪鎵嬩笂鐨勫簲鐢�
            IF is_eye_in_hand THEN
                ; S6: Move to scan pose
                ; S6: 绉诲姩鍒版媿鐓т綅濮�
                ; you should use WorkVisual to set the scan_pose,
                ; or use pendant to set the scan_pose
                ; 浣跨敤 WorkVisual 璁剧疆鎷嶇収浣嶅Э锛屾垨鑰呬娇鐢ㄧず鏁欏櫒璁剧疆鎷嶇収浣嶅Э
                ; set movel or moveJ parameters(vel, acc, zone)
                scan_pose = {X 578.80, Y -9.12, Z 541.47, A 173.34,B -4.38,C -177.88}
                xyzSetMoveJParameters(10,20,0) ; 璁剧疆鍏宠妭绉诲姩鍙傛暟: 閫熷害銆佸姞閫熷害銆佸噺閫熷害
                PTP scan_pose ; 鍏宠妭绉诲姩鍒版媿鐓т綅濮�
                ; xyzSetMoveLParameters(10,20,0) ; 璁剧疆鐩寸嚎绉诲姩鍙傛暟: 閫熷害銆佸姞閫熷害銆佸噺閫熷害
                ; LIN scan_pose ; 鐩寸嚎绉诲姩鍒版媿鐓т綅濮�

                ; S7: Send scan pose
                ; S7: 鍙戦€佹媿鐓т綅濮�
                err_code = xyzSendCurrentCartPose()
                IF (err_code <> 0) THEN  ;
                    HALT;
                    GOTO END_PROGRAM;
                ENDIF
            ENDIF

            ; S8: Request grasp pose(including cap image)
            ; S8: 璇锋眰鎶撳彇浣嶅Э
            err_code = xyzReqGraspPose(0,grasp_pose_token)
            IF (err_code <> 0) THEN  ;
                HALT;
                GOTO END_PROGRAM;
            ENDIF

            ; S9: Get grasp pose
            ; S9: 鑾峰彇鎶撳彇浣嶅Э
            err_code = xyzGetGraspPose(grasp_pose_token, grasp_pose, grasp_pose_num, obj_pipeline_num, obj_register_num, ints[])
            IF (err_code <> 0) THEN  ;
                HALT;
                GOTO END_PROGRAM;
            ENDIF

            ; 濡傛灉娌℃湁鍙互鎶撳彇鐨勭墿浣�
            IF (grasp_pose_num < 1) THEN
                WAIT SEC 5
                GOTO BEGIN_WHILE;
            ENDIF

            ; S10: Move to grasp pose and grasp object
            ; S10: 绉诲姩鍒版姄鍙栦綅濮垮苟鎶撳彇鐗╀綋
            ; Attention: Additional waypoints maybe need added
            ; 娉ㄦ剰锛氬彲鑳介渶瑕佹坊鍔犻澶栫殑璺緞鐐�
            xyzSetMoveJParameters(30,20,20)
            PTP {X 0,Y 0,Z 100,A 0,B 0,C 0}:grasp_pose C_DIS ;pre_pick 绉诲姩鍒伴鎶撳彇鐐�
            xyzSetMoveLParameters(10,20,0)
            LIN grasp_pose ; 鐩寸嚎绉诲姩鍒版姄鍙栦綅濮�
            ;$OUT[500] = TRUE; grasp object
            WAIT SEC 0.5
            xyzSetMoveLParameters(10,20,20)   
            LIN {X 0,Y 0,Z 100,A 0,B 0,C 0}:grasp_pose C_DIS ;pose_pick 鐩寸嚎绉诲姩鍒板悗鎶撳彇鐐�

            ; S11: Move to place pose and place object
            ; S11: 绉诲姩鍒版斁缃綅濮垮苟鏀剧疆鐗╀綋
            ; you should use WorkVisual to set the place_pose,
            ; or use pendant to set the place_pose
            ; 浣跨敤 WorkVisual 璁剧疆鏀剧疆浣嶅Э锛屾垨鑰呬娇鐢ㄧず鏁欏櫒璁剧疆鏀剧疆浣嶅Э
            place_pose = {X 578.80, Y -9.12, Z 541.47, A 173.34,B -4.38,C -177.88}
            xyzSetMoveJParameters(10,20,20)        
            PTP {X 0,Y 0,Z 100,A 0,B 0,C 0}:place_pose C_DIS ;pre_place 绉诲姩鍒伴鏀剧疆鐐�
            xyzSetMoveLParameters(10,20,0)
            LIN place_pose ; 鐩寸嚎绉诲姩鍒版斁缃綅濮�
            ;$OUT[500] = FALSE; place object 鏀剧疆鐗╀綋
            WAIT SEC 0.5        
            ;Attention: Additional waypoints should be added        
            xyzSetMoveLParameters(10,20,20)      
            LIN {X 0,Y 0,Z 100,A 0,B 0,C 0}:place_pose C_DIS ;post_place 鐩寸嚎绉诲姩鍒板悗鏀剧疆鐐�      

        ENDWHILE

        END_PROGRAM:
    END

搴ф爣绉诲姩浜屾瀹氫綅妯℃澘
+++++++++++++++++++++++++++++++++++++++++

.. code-block::

    DEF xyz_CartMove_reposition()
        ; S1: Initialization
        ; S1: 鍒濆鍖�
        INT err_code ; 閿欒鐮�
        INT  rough_grasp_pose_token ; 绮楀畾浣嶆姄鍙栦綅濮� token
        E6POS  rough_grasp_pose ; 绮楀畾浣嶆姄鍙栦綅濮�
        INT  rough_grasp_pose_num ; 绮楀畾浣嶆姄鍙栦綅濮挎暟閲�
        INT  board_grasp_pose_token ; 闅旀澘绮惧畾浣嶆姄鍙栦綅濮� token
        E6POS  board_grasp_pose ; 闅旀澘绮惧畾浣嶆姄鍙栦綅濮�
        INT  board_grasp_pose_num ; 闅旀澘绮惧畾浣嶆姄鍙栦綅濮挎暟閲�
        INT  fine_grasp_pose_token ; 绮惧畾浣嶆姄鍙栦綅濮� token
        E6POS  fine_grasp_pose ; 绮惧畾浣嶆姄鍙栦綅濮�
        INT  fine_grasp_pose_num ; 绮惧畾浣嶆姄鍙栦綅濮挎暟閲�
        E6POS home_pose ; home 浣嶅Э
        INT obj_pipeline_num ; 鐗╀綋杩愬姩娴佺▼缂栧彿
        INT obj_register_num ; 鐗╀綋鎶撳彇搴忓彿
        INT board_pipeline_num ; board杩愬姩娴佺▼缂栧彿
        INT board_register_num ; board 鎶撳彇搴忓彿
        INT ints[6]
        INT ints[6] 
        INT vs_id ; 瑙嗚缂栧彿
        CHAR item_codename[20] ; 鐗╀綋浠ｅ彿
        CHAR task_codename[20] ; 浠诲姟浠ｅ彿
        E6POS place_pose, scan_pose  ; 鏀剧疆浣嶅Э銆佹媿鐓т綅濮�  

        gMasterFlag = TRUE
        err_code = 0
        obj_pipeline_num = 0
        obj_register_num = 0
        board_pipeline_num = 0
        board_register_num = 0
        fine_grasp_pose_token = 0

        ; 浠ヤ笅涓轰腑鏂鐞嗙瓑浠ｇ爜锛屼笉鑳藉垹闄�
        GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( )
        INTERRUPT ON 3
        BAS (#INITMOV,0 )
        ; The first movement for Kuka must be a PTP movement
        $BWDSTART = FALSE
        PDAT_ACT = PPDAT_1
        FDAT_ACT = FPDAT_2
        BAS(#PTP_PARAMS, PPDAT_1.vel)
        ; Comment out this line if there is an error
        SET_CD_PARAMS (0)
        PTP $AXIS_ACT

        ; $OUT[500] = TRUE; io initialization IO 鍒濆鍖�

        ; S2: Connect to IPC
        ; S2: 杩炴帴 IPC
        xyzOpenSocket("MasterClient") ; 杩炴帴 IPC

        ; gStrOpSuccess = STRCLEAR(task_codename[]) ; 蹇呴』鍏堟竻绌哄瓧绗︿覆
        ; task_codename[] = "cart_repo.t"
        ; err_code = xyzSwitchTask(task_codename[]) ; 鍒囨崲浠诲姟
        ; IF (err_code <> 0) THEN  ;
        ;    HALT;
        ;    GOTO END_PROGRAM;
        ; ENDIF

        ; S3: The camera outside the robotic arm is switched to recognize the current workpiece
        ; S3: 鍒囨崲鏈烘鑷傚閮ㄧ殑鐩告満锛岃瘑鍒綋鍓嶅伐浠�
        ; camera 1 rough location, 0: vs_id, "item1": item_codename
        ; change item_codename here!
        vs_id = 0
        gStrOpSuccess = STRCLEAR(task_codename[]) ; 蹇呴』鍏堟竻绌哄瓧绗︿覆        
        item_codename[] = "item1"
        err_code = xyzSwitchItem(vs_id, item_codename[]) ; 鍒囨崲宸ヤ欢
        IF (err_code <> 0) THEN  ;
            HALT;
            GOTO END_PROGRAM;
        ENDIF

        ; S4: Move to home pose
        ; S4: 绉诲姩鍒� home 浣嶅Э
        ; Additional: should define home_pose first,
        ; you can modify home_pose with WorkVisual,
        ; or use pendant to set the home_pose
        ; 闇€瑕佸厛瀹氫箟 home 浣嶅Э锛�
        ; 鍙互浣跨敤 WorkVisual 淇敼 home 浣嶅Э锛屾垨鑰呬娇鐢ㄧず鏁欏櫒璁剧疆 home 浣嶅Э
        home_pose = {X 578.80, Y -9.12, Z 541.47, A 173.34,B -4.38,C -177.88}
        xyzSetMoveJParameters(10,20,0) ; vel, acc, zone 璁剧疆鍏宠妭绉诲姩鍙傛暟锛氶€熷害銆佸姞閫熷害銆佸噺閫熷害
        PTP home_pose ; 鍏宠妭绉诲姩鍒� home 浣嶅Э
        ; xyzSetMoveLParameters(10,20,0) ; vel, acc, zone 璁剧疆鐩寸嚎绉诲姩鍙傛暟锛氶€熷害銆佸姞閫熷害銆佸噺閫熷害
        ; LIN home_pose ; 鐩寸嚎绉诲姩鍒� home 浣嶅Э

        BEGIN_WHILE:
        WHILE TRUE
            ; S5: Request grasp pose
            ; S5: 璇锋眰鎶撳彇浣嶅Э
            err_code = xyzReqGraspPose(0,rough_grasp_pose_token) ; 璇锋眰鎶撳彇浣嶅Э
            IF (err_code <> 0) THEN  ;
                HALT;
                GOTO END_PROGRAM;
            ENDIF

            ; S6: get grasp pose
            ; S6: 鑾峰彇鎶撳彇浣嶅Э
            err_code = xyzGetGraspPose(rough_grasp_pose_token, rough_grasp_pose,  rough_grasp_pose_num,  obj_pipeline_num, obj_register_num, ints[])
            IF (err_code <> 0) THEN  ;
                HALT;
                GOTO END_PROGRAM;
            ENDIF

            ; S7: grasp board if no object exists on board
            ; S7: 濡傛灉board涓婃病鏈夌墿浣擄紝灏辨姄鍙朾oard
            IF (rough_grasp_pose_num < 1) THEN
                ; S15: The camera outside the robotic arm is switched to recognize the board
                ; S15: 鍒囨崲鏈烘鑷傚閮ㄧ殑鐩告満锛岃瘑鍒玝oard
                ; no object in workspace, should remove board first
                ; 娌℃湁鐗╀綋鍦ㄥ伐浣滃尯锛屽簲璇ュ厛绉婚櫎board
                ; switch board; O:vs_id, board: board_name
                vs_id = 0
                gStrOpSuccess = STRCLEAR(item_codename[]) ; 蹇呴』鍏堟竻绌哄瓧绗︿覆                
                item_codename[] = "board"
                err_code = xyzSwitchItem(vs_id, item_codename[]) ;鍒囨崲宸ヤ欢涓� board
                IF (err_code <> 0) THEN  ;
                    HALT;
                    GOTO END_PROGRAM;
                ENDIF

                ; S16: Request board grasp pose
                ; S16: 璇锋眰 board 鎶撳彇浣嶅Э
                err_code = xyzReqGraspPose(0,board_grasp_pose_token)
                IF (err_code <> 0) THEN  ;
                    HALT;
                    GOTO END_PROGRAM;
                ENDIF

                ; S17: Get board grasp pose
                ; S17: 鑾峰彇 board 鎶撳彇浣嶅Э
                err_code = xyzGetGraspPose(board_grasp_pose_token, board_grasp_pose,  board_grasp_pose_num,  board_pipeline_num, board_register_num, ints[])
                IF (err_code <> 0) THEN  ;
                    HALT;
                    GOTO END_PROGRAM;
                ENDIF

                ; 濡傛灉娌℃湁鍙互鎶撳彇鐨� board
                IF (board_grasp_pose_num < 1) THEN
                    ; no board exists, you can change tote
                    HALT;
                    GOTO END_PROGRAM;
                ENDIF

                ; S18: Move to board grasp pose and grasp board
                ; S18: 绉诲姩鍒� board 鎶撳彇浣嶅Э骞舵姄鍙� board
                xyzSetMoveJParameters(30,20,20)
                PTP {X 0,Y 0,Z 100,A 0,B 0,C 0}:board_grasp_pose C_DIS ;pre_pick  绉诲姩鍒伴鎶撳彇鐐�    
                xyzSetMoveLParameters(10,20,0)
                LIN board_grasp_pose
                ; $OUT[500] = TRUE; grasp board ; 鎶撳彇 board
                WAIT SEC 0.5             
                xyzSetMoveLParameters(10,20,20)
                LIN {X 0,Y 0,Z 100,A 0,B 0,C 0}:board_grasp_pose C_DIS ;post_pick  鐩寸嚎绉诲姩鍒板悗鎶撳彇鐐�           

                ; S19: Move to board place pose
                ; S19: 绉诲姩鍒� board 鏀剧疆浣嶅Э
                ; you should use WorkVisual to set the place_pose,
                ; or use pendant to set the place_pose
                ; 闇€瑕佸厛瀹氫箟 place 浣嶅Э锛屼娇鐢� WorkVisual 鎴栬€呯ず鏁欏櫒璁剧疆
                place_pose = {X 578.80, Y -9.12, Z 541.47, A 173.34,B -4.38,C -177.88} ; 鏀剧疆浣嶅Э
                xyzSetMoveJParameters(30,20,20)        
                PTP {X 0,Y 0,Z 100,A 0,B 0,C 0}:place_pose C_DIS ;pre_place    绉诲姩鍒伴鏀剧疆鐐�      
                xyzSetMoveLParameters(10,20,0)
                ; $OUT[500] = FALSE; place board ; 鏀剧疆 board                
                LIN place_pose ; 鐩寸嚎绉诲姩鍒版斁缃綅濮�
                xyzSetMoveLParameters(10,20,20)     
                LIN {X 0,Y 0,Z 100,A 0,B 0,C 0}:place_pose C_DIS ;post_place  鐩寸嚎绉诲姩鍒板悗鏀剧疆鐐�      


                ; S20: The camera outside the robotic arm is switched to recognize the current workpiece
                ; S20: 鍒囨崲鏈烘鑷傚閮ㄧ殑鐩告満锛岃瘑鍒綋鍓嶅伐浠�
                ; 0: vs_id, "item1": item_codename
                ; change the item_codename here!
                vs_id = 0
                gStrOpSuccess = STRCLEAR(item_codename[]) ; 蹇呴』鍏堟竻绌哄瓧绗︿覆                   
                item_codename[] = "item1"
                err_code = xyzSwitchItem(vs_id, item_codename[]); 鍒囨崲宸ヤ欢涓� item1
                IF (err_code <> 0) THEN  ;
                    HALT;
                    GOTO END_PROGRAM;
                ENDIF

                GOTO BEGIN_WHILE;
            ELSE

                ; S8: Move to scan pose
                ; S8: 绉诲姩鍒版媿鐓т綅濮�
                ; Additional: should define scan_pose first,
                ; you can modify scan_pose with WorkVisual or
                ; use pendant to set scan_pose
                ; 闇€瑕佸厛瀹氫箟 scan 浣嶅Э锛屼娇鐢� WorkVisual 鎴栬€呯ず鏁欏櫒璁剧疆
                scan_pose = {X 578.80, Y -9.12, Z 541.47, A 173.34,B -4.38,C -177.88}
                xyzSetMoveJParameters(10,20,0) ; 璁剧疆鍏宠妭绉诲姩鍙傛暟锛氶€熷害銆佸姞閫熷害銆佸噺閫熷害
                PTP scan_pose ; 鍏宠妭绉诲姩鍒版媿鐓т綅濮�
                ; xyzSetMoveLParameters(10,20,0) ; 璁剧疆鐩寸嚎绉诲姩鍙傛暟锛氶€熷害銆佸姞閫熷害銆佸噺閫熷害
                ; LIN scan_pose ; 鐩寸嚎绉诲姩鍒版媿鐓т綅濮�

                ; S9: Send scan pose
                ; S9: 鍙戦€佹媿鐓т綅濮�
                err_code = xyzSendCurrentCartPose()
                IF (err_code <> 0) THEN  ;
                    HALT;
                    GOTO END_PROGRAM;
                ENDIF

                ; S10: The camera on the robotic arm is switched to recognize the current workpiece
                ; S10: 鍒囨崲鏈烘鑷備笂鐨勭浉鏈猴紝璇嗗埆褰撳墠宸ヤ欢
                ; camera2 cap precise image, 1: vs_id, "item1": item_codename
                ; change the item_codename here!
                vs_id = 1
                gStrOpSuccess = STRCLEAR(item_codename[]) ; 蹇呴』鍏堟竻绌哄瓧绗︿覆                   
                item_codename[] = "item1"
                err_code = xyzSwitchItem(vs_id, item_codename[]) ; 鍒囨崲宸ヤ欢涓� item1
                IF (err_code <> 0) THEN  ;
                    GOTO END_PROGRAM;
                ENDIF

                ; S11: Request grasp pose
                ; S11: 璇锋眰鎶撳彇浣嶅Э
                err_code = xyzReqGraspPose(1,fine_grasp_pose_token)
                IF (err_code <> 0) THEN  ;
                    HALT;
                    GOTO END_PROGRAM;
                ENDIF

                ; S12: Get grasp pose
                ; S12: 鑾峰彇鎶撳彇浣嶅Э
                err_code = xyzGetGraspPose(fine_grasp_pose_token ,fine_grasp_pose, fine_grasp_pose_num, obj_pipeline_num, obj_register_num, ints[])
                IF (err_code <> 0) THEN  ;
                    HALT;
                    GOTO END_PROGRAM;
                ENDIF

                ; 濡傛灉娌℃湁鍙互鎶撳彇鐨勫伐浠�
                IF (fine_grasp_pose_num < 1) THEN
                    HALT;
                    GOTO END_PROGRAM;
                ENDIF

                ; S13: Move to grasp pose and grasp workpiece
                ; S13: 绉诲姩鍒版姄鍙栦綅濮垮苟鎶撳彇宸ヤ欢
                xyzSetMoveJParameters(10,20,20)
                PTP {X 0,Y 0,Z 100,A 0,B 0,C 0}:fine_grasp_pose C_DIS ;pre_pick 绉诲姩鍒伴鎶撳彇鐐�
                xyzSetMoveLParameters(10,20,0)
                LIN fine_grasp_pose ; 鐩寸嚎绉诲姩鍒版姄鍙栦綅濮�
                ;$OUT[500] = TRUE;
                WAIT SEC 0.5
                xyzSetMoveLParameters(10,20,20)
                LIN {X 0,Y 0,Z 100,A 0,B 0,C 0}:fine_grasp_pose C_DIS ;post_pick 鐩寸嚎绉诲姩鍒板悗鎶撳彇鐐�   

                ; S14: Move to place pose and place workpiece
                ; S14: 绉诲姩鍒版斁缃綅濮垮苟鏀剧疆宸ヤ欢
                ; you should use WorkVisual to define place_pose,
                ; or use pendant to set place_pose
                ; 闇€瑕佸厛瀹氫箟 place 浣嶅Э锛屼娇鐢� WorkVisual 鎴栬€呯ず鏁欏櫒璁剧疆
                place_pose = {X 578.80, Y -9.12, Z 541.47, A 173.34,B -4.38,C -177.88} ; 鏀剧疆浣嶅Э
                xyzSetMoveJParameters(10,20,20)
                PTP {X 0,Y 0,Z 100,A 0,B 0,C 0}:place_pose C_DIS ;pre_place  绉诲姩鍒伴鏀剧疆鐐�              
                xyzSetMoveLParameters(10,20,0)
                LIN place_pose ; 鍏宠妭绉诲姩鍒版斁缃綅濮�
                ; $OUT[500] = FALSE;
                xyzSetMoveLParameters(10,20,20)
                LIN {X 0,Y 0,Z 100,A 0,B 0,C 0}:place_pose C_DIS ;post_place 鐩寸嚎绉诲姩鍒板悗鏀剧疆鐐�  
            ENDIF
        ENDWHILE

        END_PROGRAM:
    END


3D杞ㄨ抗绉诲姩鍚屾绋嬪簭
+++++++++++++++++++++++++++++++++++++++++++++++

.. code-block::

    DEF xyz_TrajMove_Sync()

        ; S1: Initialization
        ; S1: 鍒濆鍖�
        INT err_code
        CHAR task_codename[20]
        INT vs_id
        CHAR item_codename[20]
        E6POS home_pose
        INT pipeline_num
        INT register_num
        INT way_point_num
        INT wp_type[30]
        E6AXIS joints[50]
        E6POS carts[50]

        gMasterFlag = TRUE

        GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( )
        INTERRUPT ON 3
        BAS (#INITMOV,0 )
        ; The first movement for Kuka must be a PTP movement
        $BWDSTART = FALSE
        PDAT_ACT = PPDAT_1
        FDAT_ACT = FPDAT_2
        BAS(#PTP_PARAMS, PPDAT_1.vel)

        ; Comment out this line if there is an error
        SET_CD_PARAMS (0)
        PTP $AXIS_ACT

        ; S2: Connect to IPC
        ; S2: 杩炴帴宸ユ帶鏈�
        xyzOpenSocket("MasterClient")
        gStrOpSuccess = STRCLEAR(gSendgMotMsg[])

        ; gStrOpSuccess = STRCLEAR(task_codename[]) ; 蹇呴』鍏堟竻绌哄瓧绗︿覆
        ; task_codename[] = "traj_sync.t"
        ; err_code = xyzSwitchTask(task_codename[]) ; 鍒囨崲浠诲姟
        ; IF (err_code <> 0) THEN  ;
        ;    HALT;
        ;    GOTO err_exit;
        ; ENDIF

        ; S3: Switch to the current item
        ; S3: 鍒囨崲鍒板綋鍓嶅伐浠�
        vs_id = 0
        gStrOpSuccess = STRCLEAR(item_codename[]) ; 蹇呴』鍏堟竻绌哄瓧绗︿覆    
        item_codename[] = "item1"
        err_code = xyzSwitchItem(vs_id, item_codename[]) ; 鍒囨崲宸ヤ欢
        IF (err_code <> 0) THEN  ;
            HALT;
            GOTO err_exit;
        ENDIF

        ; IO operation
        ; $OUT[500] = False
        WAIT SEC 0.5

        ; S4: Move to home pose
        ; S4: 绉诲姩鍒� home 浣嶅Э
        ; you should use WorkVisual to set the home pose,
        ; or use pendant to set the home pose
        home_pose = {X 578.80, Y -9.12, Z 541.47, A 173.34,B -4.38,C -177.88}
        xyzSetMoveJParameters(10,20,0) ; vel, acc, zone
        PTP home_pose
        ; xyzSetMoveLParameters(10,20,0)
        ; LIN home_pose

        WHILE True
        ; S5: Request Pick and place trajectory
        ; S5: 璇锋眰鎶撳彇鏀剧疆杞ㄨ抗
        vs_id = 0
        err_code = xyzReqPickPlace(vs_id)
        IF (err_code <> 0) THEN  ;
            HALT;
            GOTO err_exit;
        ENDIF

        ; S6: Get Pick in trajectory
        ; S6: 鑾峰彇鎶撳彇鍏ョ瓙杞ㄨ抗
        err_code = xyzGetPickin(0, pipeline_num, register_num, way_point_num, wp_type[], joints[], carts[])
        IF (err_code <> 0) THEN
            ; S15
            HALT;
            GOTO err_exit;
        ENDIF

        ; if tote cleared, exit
        ; 濡傛灉娌℃湁鍙互鎶撳彇鐨勭墿浣擄紝閫€鍑�
        IF (way_point_num < 1) THEN
            GOTO clear_tore_exit
        ENDIF
        ; S8 Execute Pick in trajectory
        ; S8: 鎵ц鎶撳彇鍏ョ瓙杞ㄨ抗
        xyzExecutePickInTraj(way_point_num, wp_type[], joints[], carts[])
        ; IO operation IO 鎿嶄綔
        ; $OUT[500] = False

        ; S9: Get Pick out trajectory
        ; S9: 鑾峰彇鎶撳彇鍑虹瓙杞ㄨ抗
        err_code = xyzGetPickout(0, pipeline_num, register_num, way_point_num, wp_type[], joints[], carts[])
        IF (err_code <> 0) THEN  ;
            HALT;
            GOTO err_exit;
        ENDIF
        ; S10: Execute Pick out trajectory
        ; S10: 鎵ц鎶撳彇鍑虹瓙杞ㄨ抗
        xyzExecutePickOutTraj(way_point_num, wp_type[], joints[], carts[])

        ; S11: Get Place in trajectory
        ; S11: 鑾峰彇鏀剧疆鍏ョ瓙杞ㄨ抗
        err_code = xyzGetPlacein(0, pipeline_num, register_num, way_point_num, wp_type[], joints[], carts[])
        IF (err_code <> 0) THEN  ;
            HALT;
            GOTO err_exit;
        ENDIF
        ; S12: Execute Place in trajectory
        ; S12: 鎵ц鏀剧疆鍏ョ瓙杞ㄨ抗
        xyzExecuteTraj(way_point_num, wp_type[], joints[], carts[])

        ; IO operation IO 鎿嶄綔
        ; $OUT[500] = True

        ; S13: Get Place out trajectory
        ; S13: 鑾峰彇鏀剧疆鍑虹瓙杞ㄨ抗
        err_code = xyzGetPlaceout(0, pipeline_num, register_num, way_point_num, wp_type[], joints[], carts[])
        IF (err_code <> 0) THEN  ;
            HALT;
            GOTO err_exit;
        ENDIF
        ; S14: Execute Place out trajectory
        ; S14: 鎵ц鏀剧疆鍑虹瓙杞ㄨ抗
        xyzExecuteTraj(way_point_num, wp_type[], joints[], carts[])

        ENDWHILE

        err_exit:

        clear_tore_exit:

        END


3D杞ㄨ抗绉诲姩寮傛杩愯绋嬪簭
+++++++++++++++++++++++++++++++++++++++++++++++

.. code-block::

    DEF xyz_TrajMove_Async()

        ; S1: Initialization
        ; S1: 鍒濆鍖�
        INT err_code
        CHAR task_codename[20]
        INT vs_id
        CHAR item_codename[20]
        E6POS home_pose
        INT pipeline_num
        INT register_num
        INT way_point_num
        INT wp_type[30]
        E6AXIS joints[50]
        E6POS carts[50]

        gMasterFlag = TRUE ; 琛ㄧず姝ｅ湪杩愯鏈烘鑷備富鎺�

        ; 浠ヤ笅涓轰腑鏂鐞嗙瓑浠ｇ爜锛屼笉鑳藉垹闄�
        GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( )
        INTERRUPT ON 3
        BAS (#INITMOV,0 )
        ; The first movement for Kuka must be a PTP movement
        $BWDSTART = FALSE
        PDAT_ACT = PPDAT_1
        FDAT_ACT = FPDAT_2
        BAS(#PTP_PARAMS, PPDAT_1.vel)

        ; Comment out this line if there is an error
        SET_CD_PARAMS (0)
        PTP $AXIS_ACT

        ; S2: Connect to IPC
        ; S2: 杩炴帴宸ユ帶鏈�
        xyzOpenSocket("MasterClient")
        gStrOpSuccess = STRCLEAR(gSendgMotMsg[])

        ; gStrOpSuccess = STRCLEAR(task_codename[]) ; 蹇呴』鍏堟竻绌哄瓧绗︿覆
        ; task_codename[] = "traj_async.t"
        ; err_code = xyzSwitchTask(task_codename[]) ; 鍒囨崲浠诲姟
        ; IF (err_code <> 0) THEN  ;
        ;    HALT;
        ;    GOTO err_exit;
        ; ENDIF

        ; S3: Switch to the current item
        ; S3: 鍒囨崲鍒板綋鍓嶅伐浠�
        vs_id = 0
        gStrOpSuccess = STRCLEAR(item_codename[]) ; 蹇呴』鍏堟竻绌哄瓧绗︿覆
        item_codename[] = "item1"
        err_code = xyzSwitchItem(vs_id, item_codename[]) ; 鍒囨崲宸ヤ欢
        IF (err_code <> 0) THEN  ;
            HALT;
            GOTO err_exit;
        ENDIF

        ; IO operation IO 鎿嶄綔
        ; $OUT[500] = False

        ; S4: Move to home pose
        ; S4: 绉诲姩鍒� home 浣嶅Э
        ; you should use WorkVisual to set the home_pose,
        ; or use pendant to set the home_pose
        home_pose = {X 578.80, Y -9.12, Z 541.47, A 173.34,B -4.38,C -177.88}
        xyzSetMoveJParameters(10,20,0) ; vel, acc, zone
        PTP home_pose
        ; xyzSetMoveLParameters(10,20,0)
        ; LIN home_pose

        ; S5: Request Pick and place trajectory
        ; S5: 璇锋眰鎶撳彇鏀剧疆杞ㄨ抗
        vs_id = 0
        err_code = xyzReqPickPlace(vs_id)
        IF (err_code <> 0) THEN  ;
            HALT;
            GOTO err_exit;
        ENDIF

        WHILE True

            ; S6: Get Pick in trajectory
            ; S6: 鑾峰彇鎶撳彇鍏ョ瓙杞ㄨ抗
            vs_id = 0
            err_code = xyzGetPickin(vs_id, pipeline_num, register_num, way_point_num, wp_type[], joints[], carts[])
            IF (err_code <> 0) THEN  ;
                HALT;
                GOTO err_exit;
            ENDIF

            ; if tote cleared, exit
            ; 濡傛灉娌℃湁鍙互鎶撳彇鐨勭墿浣擄紝閫€鍑�
            IF (way_point_num < 1) THEN
                GOTO clear_tore_exit
            ENDIF
            ; S8 Execute Pick in trajectory
            ; S8: 鎵ц鎶撳彇鍏ョ瓙杞ㄨ抗
            xyzExecutePickInTraj(way_point_num, wp_type[], joints[], carts[])
            ; IO operation IO 鎿嶄綔
            ; $OUT[500] = False

            ; S9: Get Pick out trajectory
            ; S9: 鑾峰彇鎶撳彇鍑虹瓙杞ㄨ抗
            vs_id = 0
            err_code = xyzGetPickout(vs_id, pipeline_num, register_num, way_point_num, wp_type[], joints[], carts[])
            IF (err_code <> 0) THEN  ;
                HALT;
                GOTO err_exit;
            ENDIF
            ; S10: Execute Pick out trajectory
            ; S10: 鎵ц鎶撳彇鍑虹瓙杞ㄨ抗
            xyzExecutePickOutTraj(way_point_num, wp_type[], joints[], carts[])

            ; S11: request next pick and place plan in advance
            ; S11: 鎻愬墠璇锋眰涓嬩竴娆℃姄鍙栨斁缃建杩�
            vs_id = 0
            err_code = xyzReqPickPlace(vs_id)
            IF (err_code <> 0) THEN  ;
                HALT;
                GOTO err_exit;
            ENDIF

            ; S12: Get Place in trajectory
            ; S12: 鑾峰彇鏀剧疆鍏ョ瓙杞ㄨ抗
            vs_id = 0
            err_code = xyzGetPlacein(vs_id, pipeline_num, register_num, way_point_num, wp_type[], joints[], carts[])
            IF (err_code <> 0) THEN  ;
                HALT;
                GOTO err_exit;
            ENDIF
            ; S13: Execute Place in trajectory
            ; S13: 鎵ц鏀剧疆鍏ョ瓙杞ㄨ抗
            xyzExecuteTraj(way_point_num, wp_type[], joints[], carts[])

            ; IO operation IO 鎿嶄綔
            ; $OUT[500] = True

            ; S14: Get Place out trajectory
            ; S14: 鑾峰彇鏀剧疆鍑虹瓙杞ㄨ抗
            vs_id = 0
            err_code = xyzGetPlaceout(vs_id, pipeline_num, register_num, way_point_num, wp_type[], joints[], carts[])
            IF (err_code <> 0) THEN  ;
                HALT;
                GOTO err_exit;
            ENDIF
            ; S15: Execute Place out trajectory
            ; S15: 鎵ц鏀剧疆鍑虹瓙杞ㄨ抗
            xyzExecuteTraj(way_point_num, wp_type[], joints[], carts[])

        ENDWHILE

        err_exit:

        clear_tore_exit:

    END