Lập trình Robot ABB IRB 1200: Từ cơ bản đến ứng dụng Real-world

MODULE MainModule
    ! Khai báo biến
    VAR robtarget home_pos;
    VAR robtarget pick_pos;
    VAR robtarget place_pos;
    
    ! Chương trình chính
    PROC main()
        ! Khởi tạo vị trí home
        home_pos := [[300, 0, 400], [1, 0, 0, 0], [0, 0, 0, 0], [9E9, 9E9, 9E9, 9E9, 9E9, 9E9]];
        
        ! Di chuyển về home
        MoveJ home_pos, v1000, z50, tool0;
        
        ! Gọi routine pick and place
        PickAndPlace;
    ENDPROC
ENDMODULE

MoveJ target_pos, v1000, z50, tool0;
! v1000: Tốc độ 1000mm/s
! z50: Zone radius 50mm (độ mượt)
! tool0: Tool coordinate system

MoveL target_pos, v500, fine, tool0;
! fine: Dừng chính xác tại điểm (không zone)

MoveC via_point, target_pos, v500, z10, tool0;

PROC PickAndPlace()
    ! Khai báo vị trí
    VAR robtarget pick_approach;
    VAR robtarget pick_pos;
    VAR robtarget place_approach;
    VAR robtarget place_pos;
    
    ! Định nghĩa vị trí pick
    pick_approach := [[400, 200, 300], [0, 1, 0, 0], [0, 0, 0, 0], [9E9, 9E9, 9E9, 9E9, 9E9, 9E9]];
    pick_pos := [[400, 200, 150], [0, 1, 0, 0], [0, 0, 0, 0], [9E9, 9E9, 9E9, 9E9, 9E9, 9E9]];
    
    ! Định nghĩa vị trí place
    place_approach := [[200, -300, 300], [0, 1, 0, 0], [0, 0, 0, 0], [9E9, 9E9, 9E9, 9E9, 9E9, 9E9]];
    place_pos := [[200, -300, 150], [0, 1, 0, 0], [0, 0, 0, 0], [9E9, 9E9, 9E9, 9E9, 9E9, 9E9]];
    
    ! ===== PICK SEQUENCE =====
    ! Di chuyển đến approach point
    MoveJ pick_approach, v1000, z50, tool0;
    
    ! Di chuyển thẳng xuống vị trí pick
    MoveL pick_pos, v300, fine, tool0;
    
    ! Đóng gripper (kích hoạt DO)
    SetDO DO_Gripper, 1;
    WaitTime 0.5;
    
    ! Nâng lên
    MoveL pick_approach, v500, z50, tool0;
    
    ! ===== PLACE SEQUENCE =====
    ! Di chuyển đến approach point
    MoveJ place_approach, v1000, z50, tool0;
    
    ! Di chuyển xuống vị trí place
    MoveL place_pos, v300, fine, tool0;
    
    ! Mở gripper
    SetDO DO_Gripper, 0;
    WaitTime 0.5;
    
    ! Nâng lên
    MoveL place_approach, v500, z50, tool0;
    
    ! Về home
    MoveJ home_pos, v1000, z50, tool0;
ENDPROC

MODULE ConveyorTracking
    ! Khai báo conveyor object
    VAR intnum conveyor_int;
    
    PROC InitConveyor()
        ! Cấu hình conveyor tracking
        CnvGenLink cnv1, "//UNIT_1/S0024/Y1_AI1", "//UNIT_1/S0024/Y2_AI2";
        
        ! Kết nối interrupt khi phát hiện sản phẩm
        CONNECT conveyor_int WITH ProductDetected;
        ISignalDI DI_ProductSensor, HIGH, conveyor_int;
    ENDPROC
    
    TRAP ProductDetected
        VAR num conv_pos;
        
        ! Lấy vị trí hiện tại trên conveyor
        CnvReadActPos cnv1, conv_pos;
        
        ! Tracking object trên conveyor
        StartMove;
        MoveL RelTool(pick_pos, 0, 0, 0), v500, fine, tool0 \WObj:=wobj_conveyor;
        
        ! Pick from moving conveyor
        SetDO DO_Gripper, 1;
        WaitTime 0.3;
        
        ! Place at fixed location
        MoveJ place_pos, v800, z20, tool0 \WObj:=wobj0;
        SetDO DO_Gripper, 0;
        StopMove;
    ENDTRAP
ENDMODULE

PROC SafePickAndPlace()
    VAR num error_code;
    
    ! Error handler
    ERROR
        IF ERRNO = ERR_ROBLIMIT THEN
            ! Robot ngoài giới hạn
            TPWrite "Error: Robot out of reach!";
            MoveAbsJ home_joints, v500, fine, tool0;
        ELSEIF ERRNO = ERR_COLL THEN
            ! Phát hiện collision
            TPWrite "Collision detected! Emergency stop.";
            StopMove;
            ClearPath;
        ENDIF
        RETURN;
    
    ! Main logic
    PickAndPlace;
ENDPROC