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

鏈烘鑷備富鎺т富鍑芥暟璇存槑
++++++++++++++++++++++++++++++++

浠ヤ笅涓烘満姊拌噦涓绘帶妯℃澘浠ｇ爜锛屾敞鎰忓宸ユ帶鏈鸿繑鍥炵殑 err_code 杩涜鍒ゆ柇銆�

.. code-block:: 

    .PROGRAM cartmovebasic()
    ;------------------------------------------------------------------------
    ;cart move 鍩虹妯℃澘
    ;------------------------------------------------------------------------
        ; S1: 鍒濆鍖栧弬鏁�
        CALL set_const
        CP ON
        MESSAGE ON
        CALL close_all_sock
        ; 鐩稿叧杩愬姩鍙傛暟闇€瑕佸悎鐞嗚缃�
        SPEED 100 ALWAYS
        ACCEL 100 ALWAYS
        ACCURACY 10 ALWAYS
        ; 鏁板瓧杈撳嚭淇″彿闇€瑕佸悎鐞嗚缃�
        SIGNAL -1
        ; 涓€浜涚ず鏁欑偣浣嶉渶瑕佸悎鐞嗚缃�
        POINT #HOME = #PPOINT(0,0,0,0,0,0)
        POINT cart_wp1 = TRANS(0,0,0,0,0,0) ; scan pose
        POINT cart_wp2 = TRANS(0,0,0,0,0,0)
        .eye_on_hand = 0 ; 鐪煎湪鎵嬩笂鏍囧織浣�

        ; S2: 杩炴帴鍒颁笂浣嶆満
        CALL xyz_conn(sock_id, IP[], PORT)

        ; CALL xyz_sw_flw(.error_code, "test.t")
        ; IF .error_code <> 0 GOTO LABEL_FAIL

        ; S3: 鍒囨崲宸ヤ欢
        CALL xyz_sw_item(.error_code, 0, "obj1")
        IF .error_code <> 0 GOTO LABEL_FAIL
        
        ; S4: 绉诲姩鍒癶ome鐐癸紝闇€瑕佹彁鍓嶈缃ソhome鐐逛綅缃�
        JMOVE #HOME
        WHILE 1 DO
            LABEL_START:
            ; S5: 鐪煎湪鎵嬩笂
            IF .eye_on_hand == 1 THEN
                ; S6: 绉诲姩鍒版媿鐓т綅
                ; you should define scan_pose first
                LMOVE cart_wp1
                POINT sending_pose = cart_wp1
                ; S7: 鍙戦€佹媿鐓т綅濮跨粰涓婁綅鏈�
                CALL xyz_s_c_pose(.error_code)
                IF .error_code <> 0 GOTO LABEL_FAIL
            END

            ; S8: 璇锋眰鎶撳彇浣嶅Э
            CALL xyz_r_g_pose(.error_code, 0, .token)
            IF .error_code <> 0 GOTO LABEL_FAIL

            ; S9: 鑾峰彇鎶撳彇浣嶅Э
            CALL xyz_g_g_pose(.error_code, .token, .pose_num, .pipe_num, .reg_num)
            IF .error_code <> 0 GOTO LABEL_FAIL
            IF .pose_num < 1 THEN
                TWAIT 5
                GOTO LABEL_START
            END
            ; S10: 绉诲姩鍒版姄鍙栦綅骞舵姄鍙�
            ; 娉ㄦ剰锛屽彲鑳介渶瑕佹坊鍔犱竴浜涜繃娓＄偣
            LMOVE grasp_pose
            SIGNAL 1

            ; S11: 绉诲姩鍒版斁缃綅骞舵斁缃�
            LMOVE cart_wp2
            SIGNAL -1
        END
    LABEL_FAIL:
        CALL xyz_disconn(.ret, sock_id)
    .END


    .PROGRAM cartmoverepo()
    ;------------------------------------------------------------------------
    ;cart move 閲嶅畾浣嶆ā鏉�
    ;------------------------------------------------------------------------
        ; S1: 鍒濆鍖栧弬鏁�
        CALL set_const
        CP ON
        MESSAGE ON
        CALL close_all_sock
        ; 鐩稿叧杩愬姩鍙傛暟闇€瑕佸悎鐞嗚缃�
        SPEED 100 ALWAYS
        ACCEL 100 ALWAYS
        ACCURACY 10 ALWAYS
        ; 鏁板瓧杈撳嚭淇″彿闇€瑕佸悎鐞嗚缃�
        SIGNAL -1
        SIGNAL -2
        ; 涓€浜涚ず鏁欑偣浣嶉渶瑕佸悎鐞嗚缃� 
        POINT #HOME = #PPOINT(0,0,0,0,0,0)
        POINT cart_wp1 = TRANS(0,0,0,0,0,0) ; scan pose
        POINT cart_wp2 = TRANS(0,0,0,0,0,0)

        ; S2: 杩炴帴鍒颁笂浣嶆満
        CALL xyz_conn(sock_id, IP[], PORT)

        ;CALL xyz_sw_flw(.error_code, "test.t")
        ;IF .error_code <> 0 GOTO LABEL_FAIL

        ; S3: 鍒囨崲宸ヤ欢   
        CALL xyz_sw_item(.error_code, 0, "obj1")
        IF .error_code <> 0 GOTO LABEL_FAIL

        ; S4: 绉诲姩鍒癶ome鐐癸紝闇€瑕佹彁鍓嶈缃ソhome鐐逛綅缃�
        JMOVE #HOME

        ; S5: 璇锋眰鎶撳彇浣嶅Э
        CALL xyz_r_g_pose(.error_code, 0, .token)
        IF .error_code <> 0 GOTO LABEL_FAIL
        
        WHILE 1 DO
            LABEL_START:
            ; S6: 鑾峰彇鎶撳彇浣嶅Э
            CALL xyz_g_g_pose(.error_code, .token, .pose_num, .pipe_num, .reg_num)
            IF .error_code <> 0 GOTO LABEL_FAIL

            ; S7:
            IF .pose_num < 1 THEN
                ;S15: 涓婁綅鏈哄垏鎹㈣嚦璇嗗埆闅旀澘
                ;宸ヤ綔绌洪棿涓凡缁忔病鏈夊伐浠讹紝闇€瑕佸厛鍙栬蛋闅旀澘
                CALL xyz_sw_item(.error_code, 0, "board")
                IF .error_code <> 0 GOTO LABEL_FAIL

                ; S16: 璇锋眰闅旀澘鎶撳彇浣嶅Э
                CALL xyz_r_g_pose(.error_code, 0, .token)
                IF .error_code <> 0 GOTO LABEL_FAIL

                ; S17: 鑾峰彇闅旀澘鎶撳彇浣嶅Э
                CALL xyz_g_g_pose(.error_code, .token, .pose_num, .pipe_num, .reg_num)
                IF (.error_code <> 0) OR (.pose_num < 1) GOTO LABEL_FAIL

                ; S18: 绉诲姩鍒版姄鍙栦綅骞舵姄鍙栭殧鏉�
                LMOVE grasp_pose
                SIGNAL 1

                ; S19: 绉诲姩鍒版斁缃綅骞舵斁缃�
                LMOVE cart_wp2
                SIGNAL -1

                ;娉ㄦ剰锛屽彲鑳介渶瑕佹坊鍔犱竴浜涜繃娓＄偣
            
                ;S20: 涓婁綅鏈哄垏鎹㈣嚦璇嗗埆宸ヤ欢
                CALL xyz_sw_item(.error_code, 0, "obj1")
                IF .error_code <> 0 GOTO LABEL_FAIL
                CALL xyz_r_g_pose(.error_code, 0, .token)
                IF .error_code <> 0 GOTO LABEL_FAIL
                GOTO LABEL_START
            ELSE
                ; S8: 绉诲姩鍒版媿鐓т綅锛岄渶瑕佸厛绀烘暀璇ユ媿鐓т綅
                LMOVE cart_wp1
                POINT sending_pose = cart_wp1

                ; S9: 鍙戦€佹媿鐓т綅濮�
                CALL xyz_s_c_pose(.error_code)
                IF .error_code <> 0 GOTO LABEL_FAIL

                ; S10: 涓婁綅鏈哄垏鎹㈣嚦璇嗗埆宸ヤ欢
                CALL xyz_sw_item(.error_code, 1, "obj1")
                IF .error_code <> 0 GOTO LABEL_FAIL

                ; S11: 璇锋眰鎶撳彇浣嶅Э
                CALL xyz_r_g_pose(.error_code, 1, .token)
                IF .error_code <> 0 GOTO LABEL_FAIL

                ; S12: 鑾峰彇鎶撳彇浣嶅Э
                CALL xyz_g_g_pose(.error_code, .token, .pose_num, .pipe_num, .reg_num)
                IF (.error_code <> 0) OR (.pose_num < 1) GOTO LABEL_FAIL

                ; S13: 绉诲姩鍒版姄鍙栦綅骞舵姄鍙�
                LMOVE grasp_pose
                SIGNAL 1

                ; S14: 绉诲姩鍒版斁缃綅骞舵斁缃�
                ; Attention: Additional waypoints should be added for place obj
                LMOVE cart_wp2
                SIGNAL -1
            END
            CALL xyz_sw_item(.error_code, 0, "obj1")
            IF .error_code <> 0 GOTO LABEL_FAIL
            CALL xyz_r_g_pose(.error_code, 0, .token)
            IF .error_code <> 0 GOTO LABEL_FAIL
        END
    LABEL_FAIL:
        CALL xyz_disconn(.ret, sock_id)
    .END


    .PROGRAM trajmovesync()
    ;------------------------------------------------------------------------
    ;traj move 鍚屾妯℃澘
    ;------------------------------------------------------------------------
        ; S1: 鍒濆鍖栧弬鏁�
        CALL set_const
        CP ON
        MESSAGE ON
        CALL close_all_sock

        ; S2: 杩炴帴涓婁綅鏈� 
        CALL xyz_conn(sock_id, IP[], PORT)

        ; CALL xyz_sw_flw(.error_code, "test.t")
        ; IF .error_code <> 0 GOTO LABEL_FAIL

        ; S3: 鍒囨崲宸ヤ欢
        CALL xyz_sw_item(.error_code, 0, "item1")
        IF .error_code <> 0 GOTO LABEL_FAIL

        ; 浠ヤ笅鍙傛暟闇€瑕佽缃悎鐞嗙殑鍒濆鍊�
        SIGNAL -1
        .error_code = 0
        .ws_id = 0

        ; S4: 绉诲姩鍒癶ome鐐癸紝闇€瑕佹彁鍓嶈缃ソhome鐐逛綅缃�
        POINT #HOME = #PPOINT(0,0,0,0,0,0)
        JMOVE #HOME

        WHILE 1 DO
            ; S5: 璇锋眰鎶撳彇鍜屾斁缃鍒�
            CALL xyz_r_picpla(.error_code, .ws_id)
            IF .error_code <> 0 GOTO LABEL_FAIL

            ; S6: 鑾峰彇鎶撳彇鍏ョ瓙杞ㄨ抗
            CALL xyz_g_picin(.error_code, .ws_id, .pose_type, .wps_num, .wps_type[], .#jnts_traj[])
            IF .error_code <> 0 GOTO LABEL_FAIL
            ; S7: 鎵ц鎶撳彇鍏ョ瓙杞ㄨ抗
            CALL xyz_exectraj(2, .pose_type, .wps_num, .wps_type[], .#jnts_traj[])
            ; S8: 鑾峰彇鎶撳彇鍑虹瓙杞ㄨ抗
            CALL xyz_g_picout(.error_code, .ws_id, .pose_type, .wps_num, .wps_type[], .#jnts_traj[])
            IF .error_code <> 0 GOTO LABEL_FAIL
            ; S9: 鎵ц鎶撳彇鍑虹瓙杞ㄨ抗
            CALL xyz_exectraj(3, .pose_type, .wps_num, .wps_type[], .#jnts_traj[])
            ; S10: 鑾峰彇鏀剧疆鍏ョ瓙杞ㄨ抗
            CALL xyz_g_plain(.error_code, .ws_id, .pose_type, .wps_num, .wps_type[], .#jnts_traj[])
            IF .error_code <> 0 GOTO LABEL_FAIL
            ; S11: 鎵ц鏀剧疆鍏ョ瓙杞ㄨ抗
            CALL xyz_exectraj(4, .pose_type, .wps_num, .wps_type[], .#jnts_traj[])
            ; S12: 鑾峰彇鏀剧疆鍑虹瓙杞ㄨ抗
            CALL xyz_g_plaout(.error_code, .ws_id, .pose_type, .wps_num, .wps_type[], .#jnts_traj[])
            IF .error_code <> 0 GOTO LABEL_FAIL
            ; S13: 鎵ц鏀剧疆鍑虹瓙杞ㄨ抗
            CALL xyz_exectraj(5, .pose_type, .wps_num, .wps_type[], .#jnts_traj[])
        END
    LABEL_FAIL:
        CALL xyz_disconn(.ret, sock_id)
    .END


    .PROGRAM trajmoveasync()
    ;------------------------------------------------------------------------
    ;traj move 寮傛妯℃澘
    ;------------------------------------------------------------------------
        ; S1: 鍒濆鍖栧弬鏁�
        CALL set_const
        CP ON
        MESSAGE ON
        CALL close_all_sock

        ; S2: 杩炴帴鍒颁笂浣嶆満
        CALL xyz_conn(sock_id, IP[], PORT)

        ; CALL xyz_sw_flw(.error_code, "test.t")
        ; IF .error_code <> 0 GOTO LABEL_FAIL

        ; S3: 鍒囨崲宸ヤ欢
        CALL xyz_sw_item(.error_code, 0, "item1")
        IF .error_code <> 0 GOTO LABEL_FAIL

        ; 浠ヤ笅鍙傛暟闇€瑕佽缃悎鐞嗙殑鍒濆鍊�
        SIGNAL -1
        .error_code = 0
        .ws_id = 0

        ; S4: 绉诲姩鍒癶ome鐐癸紝闇€瑕佹彁鍓嶈缃ソhome鐐逛綅缃�
        POINT #HOME = #PPOINT(0,0,0,0,0,0)
        JMOVE #HOME

        ; S5: 璇锋眰鎶撳彇鍜屾斁缃鍒�
        CALL xyz_r_picpla(.error_code, .ws_id)
        IF .error_code <> 0 GOTO LABEL_FAIL
        WHILE 1 DO
            ; S6: 鑾峰彇鎶撳彇鍏ョ瓙杞ㄨ抗
            CALL xyz_g_picin(.error_code, .ws_id, .pose_type, .wps_num, .wps_type[], .#jnts_traj[])
            IF .error_code <> 0 GOTO LABEL_FAIL
            ; S7: 鎵ц鎶撳彇鍏ョ瓙杞ㄨ抗
            CALL xyz_exectraj(2, .pose_type, .wps_num, .wps_type[], .#jnts_traj[])
            ; S8: 鑾峰彇鎶撳彇鍑虹瓙杞ㄨ抗
            CALL xyz_g_picout(.error_code, .ws_id, .pose_type, .wps_num, .wps_type[], .#jnts_traj[])
            IF .error_code <> 0 GOTO LABEL_FAIL
            ; S9: 鎵ц鎶撳彇鍑虹瓙杞ㄨ抗
            CALL xyz_exectraj(3, .pose_type, .wps_num, .wps_type[], .#jnts_traj[])
            ; S10: 璇锋眰涓嬩竴娆＄殑鎶撳彇鍜屾斁缃鍒�
            CALL xyz_r_picpla(.error_code, .ws_id)
            IF .error_code <> 0 GOTO LABEL_FAIL
            ; S11: 鑾峰彇鏀剧疆鍏ユ杞ㄨ抗
            CALL xyz_g_plain(.error_code, .ws_id, .pose_type, .wps_num, .wps_type[], .#jnts_traj[])
            IF .error_code <> 0 GOTO LABEL_FAIL
            ; S12: 鎵ц鏀剧疆鍏ョ瓙杞ㄨ抗
            CALL xyz_exectraj(4, .pose_type, .wps_num, .wps_type[], .#jnts_traj[])
            ; S13: 鑾峰彇鏀剧疆鍑虹瓙杞ㄨ抗
            CALL xyz_g_plaout(.error_code, .ws_id, .pose_type, .wps_num, .wps_type[], .#jnts_traj[])
            IF .error_code <> 0 GOTO LABEL_FAIL
            ; S14: 鎵ц鏀剧疆鍑虹瓙杞ㄨ抗
            CALL xyz_exectraj(5, .pose_type, .wps_num, .wps_type[], .#jnts_traj[])
        END
    LABEL_FAIL:
        CALL xyz_disconn(.ret, sock_id)
    .END