TRƯỜNG CƠ KHÍ

BÁO CÁO GIỮA KỲ ROBOT ABB

Môn: Kỹ thuật lập trình robot CN – ME4382

Lớp: Cơ điện tử - K64C
Mã lớp: 145883
GVHD: TS. Trần Thị Thanh Hải

Nhóm: 5
Sinh viên 1: Nguyễn Hoàng Đạo Mã SV: 20194938
Sinh viên 2: Nguyễn Quang Toàn Mã SV: 20195198
Sinh viên 3: Phạm Tiến Bình Mã SV: 20194916

HÀ NỘI, 12/2023
1. Đề bài tập:
2. Danh sách Input/Ouput:
Input Output
DO_00_XL_KEP
DI_00_SS_CO_PHOI
DO_00_XL_NHA

*Chú thích:
- DI_00_SS_CO_PHOI: cảm biến có phôi/không có phôi tại vị trí.
- DO_00_XL_KEP: xy lanh kẹp chặt.
- DO_00_XL_NHA: xy lanh nhả ra.
3. Sơ đồ thuật toán:
4. Chương trình lập trình:
4.1. Code hoàn chỉnh:
MODULE Module1
!***********************************************************
!
! Module: Module1
!
! Description:
! <Insert description here>
!
! Author: Nguyen Hoang Dao
!
! Version: 1.0
!
!***********************************************************
CONST robtarget AA:=[[134.74,-425.92,247.67],[0.0218059,0.00114361,0.999729,-
0.00811656],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget B:=[[136.37,-195.17,247.66],[0.021803,0.00111167,0.999728,-
0.0081574],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget C:=[[-97.27,-195.15,247.67],[0.0218385,0.00101548,0.999728,-
0.0080736],[-2,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget D:=[[-97.27,-427.96,247.67],[0.0218228,0.00102749,0.999729,-
0.00801689],[-2,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget K:=[[-78.49,-313.04,247.67],[0.0218231,0.000919155,0.99973,-
0.00793529],[-2,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget K10:=[[-45.57,-285.68,202.84],[0.0218135,0.00100728,0.999729,-
0.00804207],[-2,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget J:=[[-15.54,-312.58,202.84],[0.0218104,0.00102473,0.99973,-
0.00799725],[-2,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget E:=[[-17.43,-362.63,202.85],[0.0218091,0.00101157,0.99973,-
0.00797627],[-2,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
 CONST robtarget F:=[[85.53,-363.62,202.85],[0.0218127,0.00102457,0.99973,-
0.00798277],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget G:=[[85.53,-245.81,202.85],[0.0218202,0.00107662,0.999729,-
0.00799752],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget L:=[[37.91,-245.79,202.85],[0.021833,0.00107311,0.999729,-
0.00803101],[-1,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget L10:=[[13.62,-225.31,202.84],[0.021848,0.00107835,0.999728,-
0.00804555],[-1,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget I:=[[40.55,-204.54,202.84],[0.0218435,0.00107488,0.999728,-
0.00806663],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget M:=[[-5.44,-303.33,202.85],[0.0218367,0.00111257,0.999729,-
0.00794748],[-2,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget M10:=[[34.23,-261.46,202.85],[0.0218342,0.0010743,0.99973,-
0.00790877],[-1,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget N:=[[77.62,-299.83,202.85],[0.0218348,0.00108368,0.99973,-
0.00793164],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Ndown:=[[80.68,-300.99,83.95],[0.0215788,0.00108612,0.999735,-
0.0079619],[-1,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Jdown:=[[-18.36,-300.99,83.95],[0.0215711,0.00108468,0.999735,-
0.00796995],[-2,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget A1:=[[144.38,-397.66,83.95],[0.0215487,0.00107672,0.999736,-
0.0079178],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget B1:=[[144.50,-206.23,208.29],[0.0215493,0.00112749,0.999736,-
0.00791447],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget C1:=[[-43.98,-206.23,208.29],[0.0215542,0.00106924,0.999736,-
0.00788891],[-2,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget D1:=[[-45.03,-397.16,208.29],[0.0215301,0.00108608,0.999736,-
0.00788987],[-2,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
VAR num loopCount := 1;
!***********************************************************
!
! Procedure main
!
! This is the entry point of your program
!
!***********************************************************
PROC main()
!Add your code here
WHILE loopCount < 6 DO
MoveJ AA, v1000, fine, tool0;
MoveL B, v1000, fine, tool0;
MoveL C, v1000, fine, tool0;
MoveL D, v1000, fine, tool0;
MoveJ K, v1000, fine, tool0;
MoveC K10, J, v20, z10, tool0;
WaitTime 2;
Reset DO_00_XL_KEP;
Set DO_01_XL_NHA;
WaitTime 2;
MoveL Jdown, v1000, fine, tool0;
WaitTime 2;
Reset DO_01_XL_NHA;
Set DO_00_XL_KEP;
WaitTime 3;
MoveL J, v1000, fine, tool0;
MoveL E, v1000, fine, tool0;

IF DI_00_SS_CO_PHOI = 1 THEN
MoveL F, v1000, fine, tool0;
MoveL G, v1000, fine, tool0;
MoveL L, v1000, fine, tool0;
 MoveC L10, I, v20, z10, tool0;
MoveJ M, v1000, fine, tool0;
MoveC M10, N, v20, z10, tool0;
MoveL Ndown, v1000, fine, tool0;
WaitTime 2;
Reset DO_00_XL_KEP;
Set DO_01_XL_NHA;
WaitTime 3;
ENDIF

IF DI_00_SS_CO_PHOI = 0 THEN
MoveJ D1, v1000, fine, tool0;
MoveL A1, v1000, fine, tool0;
MoveL B1, v1000, fine, tool0;
MoveL C1, v1000, fine, tool0;
MoveJ M, v1000, fine, tool0;
MoveC M10, N, v20, z10, tool0;
MoveL Ndown, v1000, fine, tool0;
WaitTime 2;
Reset DO_00_XL_KEP;
Set DO_01_XL_NHA;
WaitTime 3;
ENDIF
TPWrite "Lan thu:";
TPWrite ValToStr(loopCount);
loopCount := loopCount + 1;
ENDWHILE
ENDPROC
ENDMODULE

4.2. Giải thích các dòng lệnh:

- Đầu tiên ta cho robot di chuyển tới các điểm được đánh dấu trên đề bài để thực
hiện việc lấy tọa độ các điểm, sau khi lưu tọa độ các điểm sẽ được thể hiện trong
chương trình dưới dạng dòng lệnh như sau:
Ví dụ với tọa độ điểm AA nằm phía trên điểm A trong đề bài:
>> CONST robtarget AA:=[[134.74,-425.92,247.67],[0.0218059,0.00114361,0.999729,-
0.00811656],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];

- Sau khi đã lấy được tọa độ các điểm ta bắt đầu lập trình để robot thực hiện theo
yêu cầu đầu bài.
- Robot sẽ di chuyển từ tọa độ Home tới điểm AA nằm phía trên điểm A trong đề
bài bằng lệnh MoveJ – di chuyển nhanh nhất, theo quỹ đạo tối ưu nhất – với vận
tốc v1000.
>> MoveJ AA, v1000, fine, tool0;

- Sau đó ta cho robot di chuyển theo quỹ đạo hình chữ nhật ABCD dử dụng lệnh
MoveL – di chuyển theo quỹ đạo là đường thẳng- với vận tốc v1000.
>> MoveL B, v1000, fine, tool0;
>> MoveL C, v1000, fine, tool0;
>> MoveL D, v1000, fine, tool0;

- Sau khi đã đến điểm D ta cho robot di chuyển đến điểm K bằng lệnh MoveJ.
>> MoveJ K, v1000, fine, tool0;

- Từ K ta sẽ di chuyển robot theo cung tròn bán kính R10 từ K đến J qua điểm
trung gian K10 với vận tốc v20 bằng lệnh Move C rồi dùng lệnh WaitTime để chờ
2s:
>> MoveC K10, J, v20, z10, tool0;
>> WaitTime 2;

- Tại điểm J ta tiến hành mở kẹp bằng 2 câu lệnh sau:

>> Reset DO_00_XL_KEP;
>> Set DO_01_XL_NHA;
- Ta chờ 2s cho kẹp mở hoàn toàn rồi di chuyển xuống điểm Jdown bằng lệnh
MoveL, vận tốc v1000 rồi đóng kẹp và gắp vật, chờ 3s để kẹp đóng chặt rồi ta cho
robot quay lại điểm J và di chuyển qua điểm E bằng đường thẳng với lệnh MoveL.
>> WaitTime 2;
>> MoveL Jdown, v1000, fine, tool0;
>> WaitTime 2;
>> Reset DO_01_XL_NHA;
>> Set DO_00_XL_KEP;
>> WaitTime 3;
>> MoveL J, v1000, fine, tool0;
>> MoveL E, v1000, fine, tool0;

- Tại E ta có 2 phương án nên ta sẽ sử dụng lệnh IF:

+ Trường hợp 1: khi có tín hiệu cảm biến tại E - DI_00_SS_CO_PHOI = 1 ta sẽ cho
robot di chuyển từ E theo đường thẳng tới F  G  L với v1000. Từ L ta di
chuyển theo cung tròn có bán kính R10 tới điểm I qua điểm trung gian I10 với vận
tốc v20. Tiếp đó ta cho robot di chuyển tới điểm M nhanh nhất bằng lệnh MoveJ,
vận tốc 1000 rồi sang N theo cung tròn bán kính R20 qua điểm trung gian M10 với
vận tốc v20.
>> IF DI_00_SS_CO_PHOI = 1 THEN
>> MoveL F, v1000, fine, tool0;
>> MoveL G, v1000, fine, tool0;
>> MoveL L, v1000, fine, tool0;
>> MoveC L10, I, v20, z10, tool0;
>> MoveJ M, v1000, fine, tool0;
>> MoveC M10, N, v20, z10, tool0;

Tại điểm N ta tiến hành hạ robot xuống điểm Ndown bằng lệnh MoveL rồi chờ 2s.
Sau đó ta cho mở kẹp và thả vật tại điểm N rồi chờ 3s cho kẹp nhả hoàn toàn và kết
thúc chương trình.
>> MoveL Ndown, v1000, fine, tool0;
>> WaitTime 2;
>> Reset DO_00_XL_KEP;
>> Set DO_01_XL_NHA;
>> WaitTime 3;
>> ENDIF

+ Trường hợp 2: khi không có tín hiệu cảm biến tại E - DI_00_SS_CO_PHOI = 0 ta sẽ
cho robot di chuyển từ E tới D1 nhanh nhất bằng lệnh MoveJ rồi đi theo hình chữ
nhật D1A1B1C1 với vận tốc v1000. Từ C1 ta di chuyển sang M nhanh nhất bằng
lệnh MoveJ, vận tốc 1000 rồi sang N theo cung tròn bán kính R20 qua điểm trung
gian M10 với vận tốc v20.
>> IF DI_00_SS_CO_PHOI = 0 THEN
>> MoveJ D1, v1000, fine, tool0;
>> MoveL A1, v1000, fine, tool0;
>> MoveL B1, v1000, fine, tool0;
>> MoveL C1, v1000, fine, tool0;
>> MoveJ M, v1000, fine, tool0;
>> MoveC M10, N, v20, z10, tool0;

Tại điểm N ta tiến hành hạ robot xuống điểm Ndown bằng lệnh MoveL rồi chờ 2s.
Sau đó ta cho mở kẹp và thả vật tại điểm N rồi chờ 3s cho kẹp nhả hoàn toàn và kết
thúc chương trình.
>> MoveL Ndown, v1000, fine, tool0;
>> WaitTime 2;
>> Reset DO_00_XL_KEP;
>> Set DO_01_XL_NHA;
>> WaitTime 3;
>> ENDIF

- Để chương trình chạy 5 lần rồi dừng ta sử dụng vòng lặp While:
+ Đầu tiên ta tạo một biến loopCount = 1 với định dạng number như sau:
>> VAR num loopCount := 1;
+ Sau đó ta tạo vòng lặp While như sau:
>> WHILE loopCount < 6 DO
>> ……………
>> loopCount := loopCount + 1;
>> ENDWHILE

Mỗi lần hoàn thành 1 lần chương trình biến loopCount sẽ tăng lên 1 đến khi đạt giá
trị là 5 tức 5 lần hoàn thành thì sẽ kết thúc vòng lặp và chương trình kết thúc.
- Để in thông báo ra màn hình mỗi lần hoàn thành chương trình ta sử dụng lệnh
TPWrite trong vòng lặp While như sau:
>> TPWrite "Lan thu:";
>> TPWrite ValToStr(loopCount);