案例/模板说明

机械臂主控主函数说明

以下为机械臂主控模板代码，注意对工控机返回的 err_code 进行判断。

.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: 移动到home点，需要提前设置好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: 移动到home点，需要提前设置好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: 移动到home点，需要提前设置好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: 移动到home点，需要提前设置好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