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

机械臂主控模板说明
++++++++++++++++++++++++++++++++

以下为机械臂主控模板代码，包含坐标移动基础模板, 坐标移动二次定位模板，轨迹移动同步模板以及轨迹移动异步模板。
关于模板中的 API 可以查阅 ABB 的 “API 说明” 部分。

**请注意模板函数并不能直接运行，请一定根据项目现场环境和流程需求进行修改。建议使用 RobotStudio 来修改。**

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

.. code-block:: 

    PROC CartMoveBasic()
    ! S1: 初始化
    VAR num err_code;           ! 错误代码
    VAR bool eye_on_hand;       ! true 表示眼在手上，false 表示眼在手外
    VAR num token;              ! 获取请求结果凭证
    VAR jointtarget home_pose;  ! home 点
    VAR robtarget scan_pose;    ! 眼在手上时，需要发送的拍照位
    VAR pose grasp_pose;        ! 抓取位姿
    VAR num cart_array{6};      ! 用于发送拍照点位姿
    VAR num pose_num;           ! 识别出的位姿个数
    VAR num pipeline_num;       ! 处理流程编号
    VAR num register_num;       ! 抓取序号

    Reset output0; ! 重置数字输出
    WaitTime 0.5;

    ! S2: 连接到工控机
    xyzCreateSocket master_socket;                                  ! 创建 socket client
    xyzConnect master_socket, master_server_ip, master_server_port; ! 连接上位机 server
    
    ! err_code := xyzSwitchTask("cart_basic.t");  ! 切换任务流图 ，默认被注释掉，如果需求可以取消注释
    ! 检查工控机返回的错误码，如果有错误程序将直接停止。可以根据项目需求自定义错误代码处理方式
    ! xyzCheckErrorCode(err_code);
    ! 重置任务
    err_code := xyzResetTask();
    xyzCheckErrorCode(err_code);

    ! S3: 切换成当前工件
    ! 切换工件，两个参数分别表示视觉服务ID，和工件代号
    ! 工件代号为MAX中映射表与通讯协议设置中的参数
    err_code := xyzSwitchItem(0,"item1"); 
    xyzCheckErrorCode(err_code);

    ! S4: 运动到 home 位
    ! 首先移动到 home 点。注意需要先定义 home_pose 的关节角度值，可以通过 RobotStudio 来修改
    ! 或者用使用其他点来替代
    MoveAbsJ home_pose, gMotSpeed, gMotZone, gMotTool \Wobj:=gMotWobj;

    ! 如果是眼在机械臂上的应用形式，请把 eye_on_hand 赋值为 true
    eye_on_hand := false;
    WHILE TRUE DO
        LOOPSTART:
        ! S5: 眼在机械臂上
        IF eye_on_hand THEN
            ! S6: 运动到拍照位姿
            ! 注意需要先定义 scan_pose 的位姿，可以通过 RobotStudio 来修改
            ! 或者用使用其他点来替代
            MoveL scan_pose, gMotSpeed, fine, gMotTool \Wobj:=gMotWobj; ! 移动到 scan pose
            ! S7: 发送拍照位姿
            err_code := xyzSendCurrentCartPose(scan_pose);
            xyzCheckErrorCode(err_code);
        ENDIF

        ! S8: 请求抓取点位
        err_code = xyzReqGraspPose(0, token);   ! 请求 grasp pose
        xyzCheckErrorCode(err_code);

        ! S9: 获取抓取点位
        err_code = xyzGetGraspPose(token, pose_num, pipeline_num, register_num, grasp_pose);
        xyzCheckErrorCode(err_code);
        IF pose_num < 1 THEN  ! 处理没有识别到工件的情况
            WaitTime 5
            GOTO LOOPSTART;
        ENDIF

        ! S10: 运动到抓取点并抓取工件
        ! 可能需要添加其他路径点作为过渡

        ! 移动到预抓取点，偏移值可以根据实际需求修改
        MoveL Offs(grasp_pose, 0, 0, 100), gMotSpeed, gMotZone, gMotTool \WObj:=gMotWobj;
        ! 移动到抓取位姿
        MoveL grasp_pose, gMotSpeed, gMotZone, gMotTool \Wobj:=gMotWobj;
        Set output0;   ! 控制数字输出
        WaitTime 0.5;

        ! S11: 运动到放置点并放置工件
        ! 移会预抓取点，偏移值可以根据实际需求修改
        MoveL Offs(grasp_pose, 0, 0, 100), gMotSpeed, gMotZone, gMotTool \WObj:=gMotWobj;

        ! 需要添加其他路径点作为过渡

        ! 移动到放置点之后
        Reset output0;  ! 重置数字输出
    ENDWHILE
    ENDPROC


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

.. code-block:: 

    PROC CartMoveRepo()
    ! S1: 初始化
    VAR num err_code;           ! 错误代码
    VAR num token;              ! 获取请求结果凭证1
    VAR num token1;             ! 获取请求结果凭证2
    VAR jointtarget home_pose;  ! home 点
    VAR robtarget scan_pose;    ! 拍照位姿
    VAR robtarget grasp_pose;   ! 抓取位姿
    VAR num cart_array{6};      ! 用于发送拍照点位姿
    VAR num pose_num;           ! 识别出的位姿个数
    VAR num pipeline_num;       ! 处理流程编号
    VAR num register_num;       ! 抓取序号

    ! S2: 连接到工控机
    xyzCreateSocket master_socket; 创建 socket
    xyzConnect master_socket, master_server_ip, master_server_port; ! 连接上位机 server

    ! err_code := xyzSwitchTask("cart_repo.t");  ! 切换任务流图 ，默认被注释掉，如果需求可以取消注释
    ! 检查工控机返回的错误码，如果有错误程序将直接停止。可以根据项目需求自定义错误代码处理方式
    ! xyzCheckErrorCode(err_code);
    ! 重置任务
    err_code := xyzResetTask();
    xyzCheckErrorCode(err_code);

    Reset output0; ! 重置数字输出
    Reset output1;
    WaitTime 0.5;

    ! S3: 机械臂外的相机切换成识别当前工件
    ! 切换工件，两个参数分别表示视觉服务 ID ，和工件代号
    ! 工件代号为 MAX 中映射表与通讯协议设置中的参数
    err_code := xyzSwitchItem(0,"item1");
    xyzCheckErrorCode(err_code);

    ! S4: 运动到 home 位
    ! 首先移动到 home 点。注意需要先定义 home_pose 的关节角度值，可以通过 RobotStudio 来修改
    ! 或者用使用其他点来替代
    MoveAbsJ home_pose, gMotSpeed, gMotZone, gMotTool \Wobj:=gMotWobj;

    WHILE TRUE DO
        ! S5: 请求工件的抓取位姿
        err_code = xyzReqGraspPose(0, token);   ! 请求抓取位姿
        xyzCheckErrorCode(err_code);

        ! S6: 获取工件的抓取位姿
        err_code = xyzGetGraspPose(token, pose_num, pipeline_num, register_num, grasp_pose);
        xyzCheckErrorCode(err_code);

        ! S7: 处理是否识别到工件
        IF pose_num < 1 THEN                       ! 处理没有识别到工件的情况
            ! 工作空间内没有工件，需要先移除隔板
            ! S15: 没有识别到工件，机械臂外的相机切换识别隔板
            err_code := xyzSwitchItem(0,"board"); ! 切换隔板
            xyzCheckErrorCode(err_code);

            ! S16: 请求隔板抓取位姿
            err_code = xyzReqGraspPose(0, token);   ! 请求隔板的抓取位姿
            xyzCheckErrorCode(err_code);

            ! S17: 获取隔板的抓取位姿
            err_code = xyzGetGraspPose(token, pose_num, pipeline_num, register_num, grasp_pose);
            xyzCheckErrorCode(err_code);
            IF pose_num < 1 THEN   ! 未识别到隔板的处理
                TPWrite "Get board grasp pose failed";
                Stop;
            ENDIF

            ! S18: 运动到隔板抓取位姿，并抓取隔板
            ! 注意: 可能需要添加其他路径点作为过渡
            ! 移动到预抓取位姿，偏移值可以根据实际需求修改
            MoveL Offs(grasp_pose, 0, 0, 100), gMotSpeed, gMotZone, gMotTool \WObj:=gMotWobj;
            ! 移动到抓取点
            MoveL grasp_pose, gMotSpeed, gMotZone, gMotTool \Wobj:=gMotWobj;
            Set output0;   ! 控制数字输出
            WaitTime 0.5;

            ! S19: 运动到隔板的放置位姿
            ! 注意: 可能需要添加其他路径点作为过渡
            ! 注意需要先定义隔板放置位姿
            ! 移回预抓取位姿，偏移值可以根据实际需求修改
            MoveL Offs(grasp_pose, 0, 0, 100), gMotSpeed, gMotZone, gMotTool \WObj:=gMotWobj;

            ! 此处需要添加放置隔板的路径点

            ! S20: 机械臂外的相机切换成识别当前工件
            err_code := xyzSwitchItem(0,"item1");  ! 切换工件
            xyzCheckErrorCode(err_code);

        ELSE    ! 如果识别数量不为0
            ! S8: 运动到拍照位姿
            ! 注意需要先定义 scan_pose 的位姿，可以通过 RobotStudio 来修改
            ! 或者用使用其他点来替代
            MoveL scan_pose, gMotSpeed, gMotZone, gMotTool \Wobj:=gMotWobj;  ! 运动到 scan pose
            
            ! S9: 发送拍照位姿
            err_code := xyzSendCurrentCartPose(scan_pose);
            xyzCheckErrorCode(err_code);
            
            ! S10: 机械臂上的相机切换成识别当前工件
            err_code := xyzSwitchItem(1,"item1");  ! 切换工作空间与工件
            xyzCheckErrorCode(err_code);
            
            ! S11: 机械臂上的相机请求工件抓取位姿
            err_code = xyzReqGraspPose(1, token1);  ! 请求抓取位姿
            xyzCheckErrorCode(err_code);
            
            ! S12: 获取工件的抓取位姿
            err_code = xyzGetGraspPose(token1, pose_num, pipeline_num, register_num, grasp_pose); ! 获取抓取位姿
            xyzCheckErrorCode(err_code);
            IF pose_num < 1 THEN  !  未识别到工件的处理
                TPWrite "Get grasp pose failed";
                Stop;
            ENDIF

            ! S13: 运动到工件的抓取位姿并进行抓取
            ! 注意: 可能需要添加其他路径点作为过渡
            ! 移动到预抓取位姿，偏移值可以根据实际需求修改
            MoveL Offs(grasp_pose, 0, 0, 100), gMotSpeed, gMotZone, gMotTool \WObj:=gMotWobj;
            ! 移动到抓取点
            MoveL grasp_pose, gMotSpeed, gMotZone, gMotTool \Wobj:=gMotWobj; 
            Set output1;   ! 控制数字输出
            WaitTime 0.5; 

            ! S14: 运动到工件的放置位姿
            ! 注意：可能需要添加其他路径点作为过渡
            ! 注意：需要定义工件放置位姿
            ! 移回预抓取位姿，偏移值可以根据实际需求修改
            MoveL Offs(grasp_pose, 0, 0, 100), gMotSpeed, gMotZone, gMotTool \WObj:=gMotWobj;

            ! 此处需要添加放置工件的路径点

            Reset output1;   ! 重置数字输出

        ENDIF
    ENDWHILE
    ENDPROC

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

.. code-block:: 

    PROC TrajMoveSync()
    ! S1: 初始化
    VAR num err_code;           ! 错误代码
    VAR num vs_id;              ! 工作空间
    VAR num pose_num;           ! 轨迹点的个数
    VAR num pipeline_num;       ! 处理流程编号
    VAR num register_num;       ! 抓取序号
    VAR num wp_type{30};
    VAR jointtarget home_pose;  ! home 点位
    VAR jointtarget joint_wp{30};
    VAR robtarget cart_wp{30};
    
    ! S2: 连接到工控机
    xyzCreateSocket master_socket;
    xyzConnect master_socket, master_server_ip, master_server_port;
    
    vs_id := 0;
    err_code := 0;
    
    ! err_code := xyzSwitchTask("traj_sync.t"); ! 切换任务流图，默认被注释掉，如果需求可以取消注释
    ! xyzCheckErrorCode(err_code);
    ! 重置任务
    err_code := xyzResetTask();
    xyzCheckErrorCode(err_code);


    ! S3: 切换成当前工件
    ! 切换工件，两个参数分别表示视觉服务 ID ，和工件代号
    ! 工件代号为 MAX 中映射表与通讯协议设置中的参数
    err_code := xyzSwitchItem(0,"item1");
    xyzCheckErrorCode(err_code);

    Reset output0;
    WaitTime 0.5;

    ! S4: 运动到 home 位
    ! 首先移动到 home 点。注意需要先定义 home_pose 的关节角度值，可以通过 RobotStudio 来修改
    ! 或者用使用其他点来替代
    MoveAbsJ home_pose, gMotSpeed, gMotZone, gMotTool \Wobj:=gMotWobj;

    WHILE TRUE DO
        ! S5: 请求抓取和放置规划
        err_code := xyzReqPickPlace(vs_id);
        xyzCheckErrorCode(err_code);
        
        ! S6: 获取 pick-in 轨迹
        err_code := xyzGetPickin(vs_id, pose_num, pipeline_num, register_num, wp_type, joint_wp, cart_wp); 
        xyzCheckErrorCode(err_code);

        ! S7: 判断是否识别到工件
        IF pose_num < 1 THEN
            TPWrite "Tote Cleared";
            Stop;
        ENDIF
        
        ! S8: 执行 pick-in 轨迹
        xyzExecuteTraj pose_num, pipeline_num, register_num, wp_type, joint_wp, cart_wp;
        Set output0;

        ! S9: 获取 pick-out 轨迹
        err_code := xyzGetPickout(vs_id, pose_num, pipeline_num, register_num, wp_type, joint_wp, cart_wp);
        xyzCheckErrorCode(err_code);
        
        ! S10: 执行 pick-out 轨迹   
        xyzExecuteTraj pose_num, pipeline_num, register_num, wp_type, joint_wp, cart_wp;    
        
        ! S11: 获取 place-in 轨迹            
        err_code := xyzGetPlacein(vs_id, pose_num, pipeline_num, register_num, wp_type, joint_wp, cart_wp); 
        xyzCheckErrorCode(err_code);

        ! S12: 执行 place-in 轨迹
        xyzExecuteTraj pose_num, pipeline_num, register_num, wp_type, joint_wp, cart_wp;                     
        Reset output0;

        ! S13: 获取 place-out 轨迹
        err_code := xyzGetPlaceout(vs_id, pose_num, pipeline_num, register_num, wp_type, joint_wp, cart_wp); 
        xyzCheckErrorCode(err_code);

        ! S14: 执行 place-out 轨迹
        xyzExecuteTraj pose_num, pipeline_num, register_num, wp_type, joint_wp, cart_wp;                      
    ENDWHILE
    ENDPROC

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

.. code-block:: 

    PROC TrajMoveAsync()
    ! S1: 初始化
    VAR num err_code;         ! 错误代码
    VAR num vs_id;            ! 工作空间 id
    VAR num pose_num;         ! 轨迹点的个数
    VAR num pipeline_num;     ! 处理流程编号
    VAR num register_num;     ! 抓取序号
    VAR num wp_type{30};
    VAR jointtarget home_pose;     ! 示教器的home点位
    VAR jointtarget joint_wp{30};
    VAR robtarget cart_wp{30};

    ! S2: 连接到工控机
    xyzCreateSocket master_socket;
    xyzConnect master_socket, master_server_ip, master_server_port;
    
    vs_id := 0;
    err_code := 0;
    
    ! err_code := xyzSwitchTask("traj_async.t"); ! 切换任务流图，默认被注释掉，如果需求可以取消注释
    ! xyzCheckErrorCode(err_code);
    ! 重置任务
    err_code := xyzResetTask();
    xyzCheckErrorCode(err_code);

    ! S3: 切换成当前工件
    ! 切换工件，两个参数分别表示视觉服务 ID ，和工件代号
    ! 工件代号为 MAX 中映射表与通讯协议设置中的参数
    err_code := xyzSwitchItem(0,"item1");
    xyzCheckErrorCode(err_code);

    Reset output0;
    WaitTime 0.5;

    ! S4: 运动到 home 位
    ! 首先移动到 home 点。注意需要先定义 home_pose 的关节角度值，可以通过 RobotStudio 来修改
    ! 或者用使用其他点来替代
    MoveAbsJ home_pose, gMotSpeed, gMotZone, gMotTool \Wobj:=gMotWobj;

    ! S5: 请求抓取和放置规划
    err_code := xyzReqPickPlace(vs_id);  ! 请求抓取和放置规划
    xyzCheckErrorCode(err_code);

    WHILE TRUE DO
        ! S6: 获取 pick-in 轨迹
        err_code := xyzGetPickin(vs_id, pose_num, pipeline_num, register_num, wp_type, joint_wp, cart_wp); 
        xyzCheckErrorCode(err_code);
        
        ! S7: 判断是否识别到工件
        IF pose_num < 1 THEN
            TPWrite "Tote Cleared";
            Stop;
        ENDIF

        ! S8: 执行 pick-in 轨迹
        xyzExecuteTraj pose_num, pipeline_num, register_num, wp_type, joint_wp, cart_wp; 
        Set output0;

        ! S9: 获取 pick-out 轨迹
        err_code := xyzGetPickout(vs_id, pose_num, pipeline_num, register_num, wp_type, joint_wp, cart_wp); 
        xyzCheckErrorCode(err_code);

        ! S10: 执行 pick-out 轨迹
        xyzExecuteTraj pose_num, pipeline_num, register_num, wp_type, joint_wp, cart_wp;      
        
        ! S11: 请求下一次的抓取和放置规划
        ! 运动到相机视野外，开始请求下一次的抓取和放置规划
        err_code := xyzReqPickPlace(vs_id);
        xyzCheckErrorCode(err_code);
        
        ! S12: 获取本次 place-in 轨迹
        ! 继续请求本次 place-in 轨迹
        err_code := xyzGetPlacein(vs_id, pose_num, pipeline_num, register_num, wp_type, joint_wp, cart_wp); 
        xyzCheckErrorCode(err_code);
        
        ! S13: 执行本次 place-in 轨迹
        xyzExecuteTraj pose_num, pipeline_num, register_num, wp_type, joint_wp, cart_wp;
        Reset output0;
        
        ! S14: 获取本次 place-out 轨迹
        err_code := xyzGetPlaceout(vs_id, pose_num, pipeline_num, register_num, wp_type, joint_wp, cart_wp); 
        xyzCheckErrorCode(err_code);
        
        ! S15: 执行本次 place-out 轨迹
        xyzExecuteTraj pose_num, pipeline_num, register_num, wp_type, joint_wp, cart_wp;
    ENDWHILE
    ENDPROC