Chương7 Lập trình dieu khien PDF

Chương 7.
LẬP TRÌNH ĐIỀU KHIỂN
7.1. Khái quát ngôn ngữ lập trình.

7.2. Cấu trúc phần mềm lập trình
7.3. Lập trình hướng đối tượng
7.4. Ngôn ngữ lập trình RAPID (Robot ABB)
1
Sơ đồ hệ thống điều khiển chung
x*, y*, z*
Phần mềm điều khiển MÁY TÍNH

VI ĐIỀU KHIỂN
q 1* q *n
ĐK1 ĐKn
Uđk1 Uđkn
BBĐ1 BBĐn
UĐ1 UĐn
Đ1 Đn
M Đ1 M Đn
TL1 TLn
E1 En
K1 K2 Kn
2
q1 qn
Sơ đồ hệ thống điều khiển chung
Bé nhí M¸y in Bµn phÝm Video Bµn

ngoµi d¹y häc phÝm
M«I trêng
lµm viÖc
Hệ điều Động học Động lực M¸y tÝnh
RB học RB
hành
I/O
Vi §K
BUS
Phần mềm
điều khiển
ĐK SECVO CB Lùc Thị giác
KĐ CS
camera Màn hình
C¶m biÕn §éng C¶m biÕn

VÞ trÝ, Tèc c¬ Lùc
®é secvo
6/9/2020 Ky thuat RB K59 ĐK-TĐH (20181) 3

7.1. Khái quát ngôn ngữ lập trình
 Mô đun phần mềm điều khiển : hàm thực hiện thuật toán ĐH , ĐLH, luật điều
khiển.
 Chương trình điều khiển thực hiện cac thông tin Người/Robot : điều khiển RB
thực hiện các nhiệm vụ công nghệ.
Hai phương pháp lập trình :

1. Lập trình dẫn đường (Teach and Playback) :
 Dẫn Robot di chuyển chậm bằng Joystick và ghi các thông tin góc quay khớp.
 Soạn thảo chương trình điều khiển robot thực hiện các chuyển động đó.
 Ưu điểm : Bộ nhớ ít, đơn giản thực hiện.
 Nhược : Khó khăn tích hợp các thông tin phản hồi vào hệ thống ĐK
2. Lập trình bằng ngôn ngữ văn bản : Dùng ngôn ngữ mô tả các chuyển động theo
công nghệ
 Lập trình đơn giản
 Lập trình hướng đối tượng Robot hoặc nhiệm vụ 4
Ngôn ngữ lập trình : 3 nhóm :
 Nhóm 1 : Ngôn ngữ lập trình robot : WARE , VAL, AML,….

 WARE (1973) : ngôn ngữ đầu tiên tại “Stanford Artifcial Intelligent Lab.” :
Ghép nối BR với hệ thống “Machine Vision”
 AL (1974) tại “ Stanford University” : Điều khiển RB nhiều tay thực hiện công
nghệ trong hệ tọa độ tay.
 VAL (1979) - Victor’s Assembly Language tại “Unimate Institute “:
Ngôn ngữ lập trình thương mai đầu tiên dùng cho RB PUMA.
Là sự kết hợp WARE và AL.
 VAL II (1984).
 AML (1982) – A Manufacturing Language : sản phẩm thương mại cho RB hãng
IBM.
 RAIL (1981) của Automatix Institute cho RB lắp ráp và hàn hồ quang
5
Ngôn ngữ lập trình : 3 nhóm :
 Nhóm 2 : Phát triển ngôn ngữ lập trình máy tính : mở rộng trợ giúp ĐK RB :
Pascal, Basic, C,….
 Nhóm 3 : Ngôn ngữ lập trình cho các máy ĐK chương trình số :
 MCL (1985) – Manufacturing Control Language : phát triển của
APT (Automatically Programmed Tooling) cho máy NC.
 HELP của General Electric Company
6
Thế hệ ngôn ngữ lập trình :
 Thế hệ thứ nhất : VAL là ngôn ngữ lập trình đầu tiên
 Sử dụng các câu lệnh và panel dạy học để lập trình.
 Là ngôn ngữ “mức chuyển động (Motion level)” : thực hiện các chuyển
động.
 Điều khiển ĐiỂM – ĐiỂM :
 Gồm các câu lệnh định nghĩa trình tự chuyển động (MOVE, WAIT,
SIGNAL,…)
 Nội suy đường thẳng.
 Bổ xung các chương trình con
 Các lệnh ghép nối sensor
 Đặc điểm :
 Chương trình mức độ phức tạp trung bình, dễ lập trình.
 Không có khả năng tính toán các phép toán số học phức tạp.
 Không có khả năng ghép nối các cảm biến phức tạp.
7
 Khả năng liên lạc với các máy tính khác hạn chế
Thế hệ ngôn ngữ lập trình :

 Thế hệ thứ hai :
 Cho phép RB hoàn thành các công việc phức tạp hơn.
 Là ngôn ngữ lập trình cấu trúc : AML, RAIL, MCL, VALII,….
 Tương tự như lập trình máy tinh.
 Đặc điểm :
 Điều khiển chuyển động tương tự thế hệ 1, thêm các dạng hình hoặc phức
tạp.
 Khả năng ghép nối các cảm biến nâng cao : sử dụng t/h tương tự và số nhị
phân.
 Có mức đô thông minh hơn :
 Làm việc với môi trường linh hoạt hơn bằng sử dụng thông tin về môi
trường.
 Thông tin và xử lí dữ liệu, liên lạc với máy tính, lưu trữ , báo cáo,…
 Điều khiển các cơ cấu linh hoạt : tay gắp có cảm biến lực.
 Có khả năng thích ứng với các sự kiện không bình thường
8
 Có khả năng giao tiếp với các hệ thống có cài đặt máy tính.
7.2. Cấu trúc phần mềm
+ Phần mềm gồm 3 phần tương ứng 3 mức.

+ Panel lập trình : nạp chương trình vào MT.
+ Máy tính truyền chương trình và lưu vào bộ nhớ của bộ ĐK robot.
+ Máy tính đọc dữ liệu từ các cảm biến thông qua các cổng nối tiếp hoặc song
song 9
B=
MÔ TẢ ĐỐI TƯỢNG
Mô tả đối tượng trong không gian : ma trận phép biến đổi và phương trình Động
học
Ma trận phép biến đổi đối tượng : zA

kB
{B}
p
jB
iB
{A} yA
xA
Ngôn ngữ phần mềm gồm :

+ Kiểu dữ liệu
+ Các hàm tinh toán vector, ma trận.
+ Các hàm tính toán quĩ đạo chuẩn.
+ Các hàm xác định hệ tọa độ
+ Tập lệnh 10
B=
VÍ DỤ CẤU TRÚC DỮ LIỆU CƠ BẢN

(ví dụ như ngôn ngữ lập trình PASCAL)
1. Đại lượng vô 3. Phép quay 5. Phép biến đổi

hướng Type Type
Type Rotation = record transform = record
num = real axis : vector n,o,a : vector
end; angles : num end;
matrix : rotmatrix
2. Vector : end; 6. Khung tọa độ
Type 4. Rotmatrix Type
vector = record Type frame = record
x,y,z : num rotmatrix = record rot : rotation
end; n,o,a : vector transl : vector
end; end;
11
7.3. Lập trình hướng đối tượng
Hai dạng :
+ Lập trình hướng đối tượng robot
+ Lập trình hướng đối tượng nhiệm vụ
Lập trình hướng đối tượng robot :

 Nhiệm vụ lắp ráp mô tả như là trình tự của các chuyển động (hành động)
robot
 Mỗi lệnh tương ứng với một chuyển động RB được điều khiển
Lập trình hướng đối tượng nhiệm vụ :

 Nhiệm vụ lắp ráp mô tả như là trình tự các vị trí RB phải đạt đến.
 Không mô tả tường minh các chuyển động
12
7.3. Lập trình hướng đối tượng Robot
Các bước xây dựng chương trình :
1. Xác định không gian làm việc và các bộ phận : đồ gá, giá cấp vật liệu.
2. Định nghĩa hướng , vị trí của các bộ phận bằng sử dụng cấu trúc dữ liệu.
3. Mô tả công việc bằng trình tự các hành động : di chuyển, qắp chi tiết, lắp ráp,…
4. Các lệnh điều khiển cảm biến bổ xung : phát hiện sự cố , ghi lại quá trình LV.
5. Kiểm tra chương trình và hiệu chỉnh hoàn thiện chương trình.
13
Bước 2. Định nghĩa hướng , vị trí của các bộ phận bằng sử dụng cấu trúc dữ
liệu : Mô tả vị trí, hướng đối tượng bằng ma trận đồng nhất (4x4)-Phép biến đổi
Các phép biến đổi
R: Thân RB so với {U}
T6 : Cô tay so với {R}
R
R E : Điểm kẹp so với
U
U Đ
{T6}
Đ
G : Giá đỡ so với {U}
T6 CT : chi tiết so với {G}
T6
ECT : điểm kẹp trên {
CT}
EE M : Máy so với {U}
M Đ : Vị trí lắp so với {M}
M
ECT
ECT
PBĐ so với KTĐ gốc :
TH=R.T6
CT
CT TE=R.T6.E
TCT=G.CT
G TECT=TCT.ECT
14
TĐ=M.Đ
3. Mô tả công việc bằng trình tự các hành động

Trình tự làm việc :
Di chuyển cổ tay
+ Di chuyển đến A.
+ DI chuyển đến vị trí
R gắp chi tiết
+ Gắp chi tiết
U Đ
+ Di chuyển đến B
T6
+ Di chuyển đến C
+ Di chuyển đến D.
+ Lắp chi tiết vào lỗ
E
Dùng các hàm thực
M hiện chuyển động
ECT
CT
15
7.4. Ngôn ngữ lập trình RAPID (Robot ABB)
+ Cấu trúc dữ liệu chính
+ Các hàm tính toán động học.
+ Các hàm mô tả chuyển động
16
Cấu trúc dữ liệu
1. Giá trị Số : num

Ví dụ : VAR num reg1;
...
reg1 := 3;
2. Biểu diễn vị trí : pos – ( vị trí X, Y và Z)
x, y, z : num
Ví dụ : VAR pos pos1;
...
pos1 := [500, 0, 940];
17
3. Biểu diễn hướng : orient – (mô tả hướng cổ tay và phép quay - q1, q2, q3, q4)
q1, q2, q3, q4 : num
Dấu q1, q2, q3
q2=sign(jz – ky )
q3=sign(kx – iz )
q4=sign(iy – jx )
Ví dụ 1 : VAR orient orient1;

orient1 := [1, 0, 0, 0];
18
Cấu trúc dữ liệu Ngôn ngữ RAPID (Robot ABB)
Ví dụ 2 : Xác định phép biểu diễn hướng của cổ tay
Ma trận quay
Tính được : q1=0,707

q2=0
q3=0,707
q4=0
19
Ví dụ 3 : Xác định phép biểu diễn hướng của đối tượng so với KTĐ cổ tay
Ma trận quay :
Tính được : q1=0,965926

q2=0
q3=0,258819
q4=0
20
4. Biểu diễn phép biến đổi : pose – (mô tả phép biến đổi)
trans (vecto tịnh tiến) : pos
rot (phép quay) : orient
Ví dụ : VAR pose frame1;
...
frame1.trans := [50, 0, 40];
frame1.rot := [1, 0, 0, 0];
5. Biểu diễn vị trí đích : robtarget

robconf (cấu hình robot) : confdata
extax () : extjoint
21
5. Biểu diễn vị trí đích : robtarget

robconf (cấu hình robot) : confdata
extax (Khớp bên ngoài) : extjoint
VÍ DỤ : CONST robtarget p15 := [ [600, 500, 225.3], [1, 0, 0, 0], [1, 1, 0, 0],
[ 11, 12.3, 9E9, 9E9, 9E9, 9E9] ]
extax : extjoint
robconf : confdata eax_a : num
cf1 : num (Trục 1) eax_b : num
cf4 : num (Trục 4) eax_c : num
cf6 : num (Trục 6) eax_d : num
cfx : num (Chọn ) eax_e : num
eax_f : num
22
Confdata : Dữ liệu cấu hình robot

 Dùng để biểu diễn các cấu hinh khớp khác nhau ứng với một cấu hình của cổ
tay :
Ví dụ robot 2 thanh nối : BT ngược - có hai cấu hình khác nhau ứng với một cấu
hình cổ tay.
 Confdata gồm 3 giá trị của các góc quay thứ tự 90o từ vị trí ban đầu của 3 trục
1, 4, 6
(cf1 - Trục 1 , cf4 – Trục 4 , cf6 – trục 6)
 Giá trị 0 : tương ứng góc quay : 0 - 90o
 Giá trị 1 : tương ứng góc quay : 900 - 1800
 Giá trị -1 : tương ứng góc quay : 0 - -90o
 Giá trị -2 : tương ứng góc quay : -900 - -1800
 Giá trị -3: tương ứng góc quay : -1800 - -2700
23
Confdata : Dữ liệu cấu hình

Confdata gồm 3 giá trị của các góc quay thứ tự 90o từ vị trí ban đầu của 3
trục 1, 4, 6
(cf1 - Trục 1 , cf4 – Trục 4 , cf6 – trục 6)
 Giá trị 0 : tương ứng góc quay : 0 - 90o
 Giá trị -1 : tương ứng góc quay : 0 - -90o
 Giá trị -3: tương ứng góc quay : -1800 - -2700
 Giá trị -4 : tương ứng góc quay : 2700 - -3600
VÍ DỤ VAR confdata conf15 := [1, -1, 0]
24
extjoint : Vị trí của các khớp bên ngoài :

 Dùng để biểu diễn vị trí của 6 khớp bên ngoài hoặc các cơ cấu của không gian
làm việc extjoint gồm 6 giá trị tương ứng vị trí : a, b, c, d, e, f
 Khớp quay : Góc quay (rad )
 Khớp tịnh tiến : dịch chuyển tịnh tiến (mm) .
 Khớp nào không được kết nối : Giá trị 9E9
VÍ DỤ : VAR extjoint axpos10 := [ 11, 12.3, 9E9, 9E9, 9E9, 9E9]
25
6. Biểu diễn vị trí khớp đích : jointtarget (Vị trí Khớp phải đạt tới)
robax (Vị trí khớp) : robjoint
( rax_1, rax 2 , rax_3 , rax_4 , rax_5 , rax_6 : num)
extax (Khớp bên ngoài) : extjoint
( eax_1, eax 2 , eax_3 , eax_4 , eax_5 , eax_6 : num)
VÍ DỤ : CONST jointtarget calib_pos := [ [ 0, 0, 0, 0, 0, 0], [ 0, 9E9, 9E9, 9E9,

9E9, 9E9] ]
26
7 . Dữ liệu tốc độ : speeddata : Tốc độ chuyển động của robot và các trục bên
ngoài gồm cả tốc độ chuyển động tịnh tiến và quay
v_tcp :num (mm/s) : Tốc độ của TCP
v_ori : num (độ/s) : Tốc độ quay TCP
v_leax : Tốc độ dài của các trục bên ngoài (mm/s)
v_reax : Tốc độ quay trục bên ngoài (độ/s)
VÍ DỤ : VAR speeddata vmedium := [ 1000, 30, 200, 15 ];
27
8 . Dữ liệu vị trí dừng : stoppointdata : xác định điểm dừng gần điểm lập trình
trước khi một lệnh mới.
type : loại điểm dừng : 1 inpos ; 2 stoptime ; 3 fllwtime
progsynch : đồng bộ hóa chương trình : true; fall
inpos .position : : Vị trí dừng của TCP : mức độ hội tụ vị trí (% )của vị trí dừng đặt
trước.
inpos.speed : Tốc độ tại điểm dừng : mức độ hội tụ (% tốc độ) tại vị trí dừng
đặt trước .
inpos.mintime : Thời gian đợi nhỏ nhất trước khi đạt đến vị trí. (max 20 s)
inpos.maxtime : Thời gian đợi lớn nhất trước khi đạt đến vị trí. (max 20 s)
stop time : Thời gian dừng của TCP trước khi thực hiện chuyển động tiếp theo
follow time : Thời gian chuyển đông bám theo băng (0-20s)
signal : string – dự phòng
28
checkvalue : num : dự phòng
8 . Dữ liệu vị trí dừng : stoppointdata : xác định điểm dừng gần điểm lập trình
trước khi một lệnh mới.
VÍ DỤ : VAR stoppointdata my_inpos := [ inpos, TRUE, [ 25, 40, 0.1, 5], 0,0, "", 0, 0];
MoveL *, v1000, fine \Inpos:=my_inpos, grip4;
+ Dạng điểm dừng : inpos
+ Điểm dừng đồng bộ với chương trình RAPID
+ Múc độ hội tụ : 25% của khoảng cách đối với điểm dừng.
+ Múc độ tốc độ : 40% của tốc độ đối với điểm dừng.
+ Thời gian đợinhỏ nhất trước khi hội tụ là 0,1 s
+ Thời gian đợi lớn nhất trước khi hội tụ là 5 s
Cánh tay RB sẽ di chuyển cho đến khi đạt được một trong tiêu chuẩn vị trí và tốc độ
. 29
9 . Dữ liệu tool : tooldata : mô tả đặc tính của tool : súng hàn hoặc gripper
robhold : bool : true – robot có tool; fall – không có tool
tframe (KTĐ tool) : pose (vị trí tool :x,y,z và định hướng : q1, q2, q3, q4)
tload (tải trọng tool) : loaddata
+ Khối lượng của tool, kg.
+ Trọng tâm của tải tool (x, y, z) trong KTĐ
cổ tay.
+ Mô men quán tính của tool tương đối
với trọng tâm xung quanh các trục KTĐ
tool.
30
9 . Dữ liệu tool : tooldata : mô tả đặc tính của tool : súng hàn hoặc gripper
VÍ DỤ : PERS tooldata gripper := [ TRUE, [[97.4, 0, 223.1], [0.924, 0, 0.383 ,0]], [5,
[23, 0, 75],
[1, 0, 0, 0], 0, 0, 0]];
+ Cổ tay có tool.
+ Vị trí tool : X=97,4 mm; Z = 223.1 mm trong KTĐ cổ tay
+ Trục X, Z quay xung tương đối so với hệ tọa cổ tay một góc 45o
+ Khối lượng tool : 5 kg.
+ Trọng tâm : : X=23 mm; Z = 75 mm trong KTĐ cổ tay
+ Các mô men quán tính bằng không
31
11 . Dữ liệu wobjdata - Work object data – Mô tả đối tượng RB hàn, lắp ráp, xử lý
hoặc chuyển động
robhold : bool – True (RB đang gắp đối tượng); Fall (RB không gắp vật, có tool)
ufprog : bool – Hệ thống tọa độ cố định có được sử dụng
True : Hệ thống tọa độ cố định được sử dụng.
Fall : Hệ thống tọa độ di chuyển được sử dụng
ufmec : string – Cơ cấu cơ khí RB chuyển động cùng
uframe : pose – KTĐ của người sử dụng
oframe : pose – KTĐ đối tượng
32
11 . Dữ liệu wobjdata - Work object data – Mô tả đối tượng RB hàn, lắp ráp, xử lý
hoặc chuyển động
33
2. Định nghĩa hướng , vị trí của các bộ phận bằng sử dụng cấu trúc dữ liệu :
(NGÔN NGỮ RAPID)
Các phép biến đổi

Ngôn ngữ RAPID : Biểu diễn ma trận đồng nhất
R: Thân RB so với {U}
VAR pose frame1; T6 : Cô tay so với {R}
... E : Điểm kẹp so với
frame1.trans := [50, 0, 40]; {T6}
frame1.rot := [1, 0, 0, 0]; G : Giá đỡ so với {U}
CT : chi tiết so với {G}
M : Máy so với {U}
Trans (translation ) – Đ : Vị trí lắp so với {M}
Kiểu Dữ liệu pos = [x, y, z]
rot (rotation )-
Kiểu dữ liệu orient = [q1, q2, q3, q4]
34
Hàm tính toán động học vị trí
1. CalcRobT : Hàm tính ĐHT (ma trận biểu diễn vị trí và hướng cổ tay từ vị trí khớp)
 Tính dữ liệu robtarget từ dữ liệu jointtarget.
 Kết quả trả về : Vị trí (X, Y, Z); hướng (q1, q2, q3, q4); cấu hình khớp RB và
các trục bên ngoài.
 jointtarget được biểu diễn trong calibration coordinate system
 robtarget được biểu diễn trong outermost coordinate system
CalcRobT ( Joint_target Tool [\WObj] )

Joint_target : jointtarget
Tool : tooldata (dùng cho tính toán vi trí trí RB)
[\WObj] : wobjdata
VÍ DỤ : VAR robtarget p1;

CONST jointtarget jointpos1 := [...];
p1:= CalcRobT(jointpos1, tool1 \WObj:=wobj1);
35
Hàm tính toán động học vị trí
2. CalcJointT : Hàm tính ĐHN (Tính toán vị trí khớp từ ma trận biểu diễn vị trí và
hướng cổ tay)
 Tính dữ liệu jointtarget từ dữ liệu robtarget
 Kết quả trả về : Vi trí khớp và các trục bên ngoài.
 jointtarget được biểu diễn trong calibration coordinate system
 robtarget được biểu diễn trong outermost coordinate system
CalcJointT ( [\UseCurWObjPos] Rob_target Tool [\WObj] )

[\UseCurWObjPos] : switch
Rob_target : Robttarget
Tool : tooldata (dùng cho tính toán vi trí trí RB)
[\WObj] : wobjdata
VÍ DỤ : VAR jointtarget jointpos2;

CONST robtarget p2 := [...];
jointpos2 := CalcJointT(\UseCurWObjPos, p2, tool2 \WObj:=orb1);
36
PoseInv (Pose Invert) Nghich đảo ma trận PBD
VAR pose pose1;

VAR pose pose2;
...
pose2 := PoseInv(pose1);
PoseMult (Pose Multiply) : Nhân hai ma trận PBD
PoseMult (Pose Multiply) : Nhân hai ma trận PBD
VAR pose pose1;

VAR pose pose2;
VAR pose pose3;
...
pose3 := PoseMult(pose1, pose2);
PoseVect (Pose Vector) Biến đổi một vetor
VAR pose pose1;

VAR pos pos1;
VAR pos pos2;
...
...
pos2:= PoseVect(pose1, pos1);
EulerZYX (Euler ZYX rotations) : TÍnh góc Euller từ cấu hình cổ tay
EulerZYX (Euler ZYX rotations) : TÍnh góc Euller từ cấu hình cổ tay
VAR num anglex;

VAR num angley;
VAR num anglez;
VAR pose object;
...
...
anglex := EulerZYX(\X, object.rot);
angley := EulerZYX(\Y, object.rot);
anglez := EulerZYX(\Z, object.rot);
Hàm di chuyển
1. MoveJ : Hàm di chuyển bằng các khớp đến vị trí đích với đường phi tuyến, các trục
đồng thời chuyển động đến vị trí đặt tại cùng thời điểm.
MoveJ [\Conc] ToPoint [\ID] Speed [\V] | [ \T] Zone [\Z] [\Inpos] Tool [\WObj]
]
[ \Conc ] : switch (Tùy chọn)
ToPoint : robtarget (KTĐ Đích)
[ \ID ] : identno (đồng bộ trong hệ thống nhiều cơ cấu)
Speed : speeddata (Tốc độ d/c TCP, tốc độ quay của tool, tốc độ d/c và quay
của trục ngoài)
[ \V ] : num (Tốc độ TCP – mm/s)
[ \T ] : num (Thời gian chuyển động)
zone : zonedata
[ \Z ] : num (Sử dụng cho vị trí chính xác của TCP)
[ \Inpos ] : stoppointdata (sử dụng để dừng RB tại vị trí đặt trước khi thực hiện
d/c mới)
Tool : tooldata (mô tả đặc tính của tool)
41
[\WObj] : wobjdata (mô tả đặc tính của đối tượng)
Hàm di chuyển
1. MoveJ : Hàm di chuyển bằng các khớp đến vị trí đích với đường phi tuyến
MoveJ [\Conc] ToPoint [\ID] Speed [\V] | [ \T] Zone [\Z] [\Inpos] Tool [\WObj] ]
VÍ DỤ : 1. MoveJ p1, vmax, z30, tool2;

2. MoveJ *, vmax \T:=5, fine, grip3;
42
Hàm di chuyển
2. MoveJDO : Hàm di chuyển TCP bằng d/c khớp đến vị trí đích theo đường phi tuyến với tạo
tín hiệu ra.
MoveJDO ToPoint [\ID] Speed [\T] Zone Tool [\WObj] Signal Value
Speed : speeddata
zone : zonedata
Signal : signaldo (dạng tín hiệu ra)
Value : dionum (Giá trị tín hiệu ra)
43
Hàm di chuyển
2. MoveJDO : Hàm di chuyển TCP bằng d/c khớp đến vị trí đích theo đường phi tuyến
với tạo tín hiệu ra.
MoveJDO ToPoint [\ID] Speed [\T] Zone Tool [\WObj] Signal Value
VÍ DỤ :
44
Hàm di chuyển
3. MoveJSync : Hàm di chuyển nhanh TCP đến vị trí đích theo đường cong và thực
hiện CT con tại điểm giữa của góc đường di chuyển.
MoveJSync ToPoint [\ID] Speed [\V] | [ \T] Zone [\Z] Tool [\WObj] [\Corr]
Procname
Speed : speeddata
zone : zonedata (kích thước của góc quĩ đạo chuyển động)
ProcName : string
45
Hàm di chuyển
3. MoveJSync : Hàm di chuyển nhanh TCP đến vị trí đích theo đường cong và thực
hiện CT con tại điểm giưa của góc đường di chuyển.
MoveJSync ToPoint [\ID] Speed [\V] | [ \T] Zone [\Z] Tool [\WObj] [\Corr]
Procname
VÍ DỤ : 1. MoveJSync p1, vmax, z30, tool2, "proc1";

Thực hiện chương trình tên proc1 tại điểm giữa của góc đường di
chuyển.
2.
46
Hàm di chuyển
4. MoveL : Hàm di chuyển TCP đến vị trí đích theo đường thẳng
MoveL [\Conc] ToPoint [\ID] Speed [\V] | [ \T] Zone [\Z] [\Inpos] Tool [\WObj]
[\Corr]
Speed : speeddata (Tốc độ d/c TCP, tốc độ quay của tool, tốc độ d/c và quay
của trục ngoài)
[ \Inpos ] : stoppointdata (sử dụng để dừng RB tại vị trí đặt trước khi thực hiện
d/c mới)
47
[ \Corr ] : switch
Hàm di chuyển
5. MoveLDO : Hàm di chuyển TCP đến vị trí đích theo đường thẳng với tạo tín hiệu ra
MoveLDO ToPoint [\ID] Speed [\V] | [ \T] Zone [\Z] Tool [\WObj] [\Corr] signal
Value
Speed : speeddata
Signal : signaldo (dạng tín hiệu ra)
Value : dionum (Giá trị tín hiệu ra)
48
Hàm di chuyển
5. MoveLSync : Hàm di chuyển TCP đến vị trí đích theo đường thẳng và thực hiện CT
con tại điểm giưa của góc đường di chuyển.
MoveLSync ToPoint [\ID] Speed [\V] | [ \T] Zone [\Z] Tool [\WObj] [\Corr]
Procname
Speed : speeddata
ProcName : string
49
Hàm di chuyển
5. MoveLSync : Hàm di chuyển TCP đến vị trí đích theo đường thẳng và thực hiện CT
con tại điểm giưa của góc đường di chuyển.
MoveLSync ToPoint [\ID] Speed [\V] | [ \T] Zone [\Z] Tool [\WObj] [\Corr]
Procname
Ví dụ :
50
Hàm di chuyển
6. MoveJSync : Hàm di chuyển TCP bằng d/c khớp đến vị trí đích theo đường phi
tuyến và thực hiện CT con
MoveJSync ToPoint [\ID] Speed [\T] Zone Tool [\WObj] ProcName

Speed : speeddata
zone : zonedata
ProcName : string
51
Hàm di chuyển
7. MoveAbsJ : Hàm di chuyển với vị trí khớp tuyệt đối với nội suy góc khớp, các trục
chuyển động tới vị tri đích cùng một thời điểm
MoveAbsJ [\Conc] ToJointPos [\ID] [\NoEOffs] Speed [\V] | [\T] Zone [\Z] [\Inpos]
Tool [\WObj ]
ToJointPos : jointtarget (vị trí Đích của khớp)
Speed : speeddata
[ \Inpos ] : stoppointdata
[\WObj] : wobjdata (mô tả đặc tính của đối tượng) 52
Hàm di chuyển
7. MoveAbsJ : Hàm di chuyển với vị trí khớp tuyệt đối
MoveAbsJ [\Conc] ToJointPos [\ID] [\NoEOffs] Speed [\V] | [\T] Zone [\Z] [\Inpos] Tool
[\WObj ]
VÍ DỤ : 1. MoveAbsJ p50, v1000, z50, tool2;
RB với Tool2 di chuyển đến vị trí khớp P50 (500), tốc độ 1000mm/ph, vùng 50
2. MoveAbsJ *, v1000\T:=5, fine, grip3;
RB với gripper3 di chuyển với tốc độ 1000 mm/ph trong t/g 5 s
3. MoveAbsJ *, v2000\V:=2200, z40 \Z:=45, grip3;
RB với gripper 3 di chuyển với tốc độ 2000 mm/ph và vùng 40; tốc độ 2200
mm/ph với vùng 45
53
Hàm di chuyển
8. StopMove : Hàm dừng chuyển động robot và các trục ngoài. Khi có lệnh StarMove,
Các
chuyển động được thực hiện trở lại
StopMove [\Quick] [\AllMotionTasks]

[\Quick] : switch – Dừng RB nhanh có thể.
KHông có tham số : dừng nhanh với khoảng cách dừng
dài hơn.
[\AllMotionTasks] : switch - Dừng tất cả các cơ cấu cơ khí
VÍ DỤ : 1. StopMove;
WaitDI ready_input, 1;
StartMove;
54
Hàm di chuyển
9. StartMove : Hàm chuyển động robot và các trục ngoài. Khi có lệnh StartMove, Các
chuyển động được thực hiện trở lại
+ Đặt sau các lệnh StopMove và Storepath
StartMove
VÍ DỤ : 1. StopMove;
WaitDI ready_input,1;
StartMove;
55
Hàm di chuyển
10. StorePath : Lưu trữ quĩ đạo khi xảy ra lệnh ngắt cho sử dụng tiếp theo
+ Đặt sau các lệnh StopMove và Storepath
StorePath
11. RestorePath : Sử dụng lại quĩ đạo chuyển động đã được lưu bằng lệnh StorePath
56
Thực hành : Viết chương trình điều khiển
Viết chương trình mô tả sự hoạt động của robot sau
1. Viết ma trận đồng

nhất biểu diễn KTĐ TCP
ở vị trí ban đầu và ở vị
trí B khi gắp chi tiết.
2. Viết chương trình tính
phép biến đổi biểu diễn
chi tiết so với KTĐ gốc
(BASE).
3. Viết chương trình mô
tả RB di chuyển từ Vị trí
A đến gắp vật ở điểm B
với cấu hình TCP như
trên.
DỮ LiỆU :
Vecto mô trí của TCP (Tool Center Point(OT) là :
Tại B, cấu hình cổ tay được định hướng bằng ma trận quay :
Vị trí gốc các KTĐ đối tượng : OG [150, 50, 0]T
Độ cao của chi tiết : 50
58
Viết chương trình điều khiển
Vị trí điểm E : [200,100,150]T
KTĐ của TCP ở vị trí ban đầu :
KTĐ của TCP ở vị trí B :
59
// Khai báo các phép biến đổi - cấu trúc dữ liệu

VAR pose TCPA; // KTĐ TCP ở vị trí A
VAR pose TCPB; // KTĐ TCP ở vị trí B
VAR pose TOCTG; // KTĐ chi tiết so với KTĐ OG
VAR pose TOCTO; // KTĐ chi tiết so với KTĐ gốc
VAR pose TOG; // KTĐ OG
VAR robtarget pA;
VAR robtarget pB;
//Nhập dữ liệu các phép biến đổi

TCPA.trans := [333,0.02,699]; TCPA.rot := [0.707, 0, 0.707, 0];
TCPB.trans := [200,100,150]; TCPB.rot := [0, 0, 0, 1];
TOG.trans := [150,50,0]; TOG.rot :=[1, 0, 0, 0];
TOCTG.trans := [50,50,100]; TOCTG.rot := [1, 0, 0, 0];
pA.trans := [333,0.02,699]; pA.rot := [0.707, 0, 0.707, 0]; pA.robconf:=[1,1,1,1]
pB.trans := [200,100,150]; pB.rot := [0, 0, 0, 1]; pB.robconf:=[1,1,1,1]
// Phép biến đổi biểu diễn chi tiết so với KTĐ gốc (BASE).
TOCTO:= PoseMult(TOG, TOCTG);
//Chương trình di chuyển TCP từ vị trí A đến B
MoveJ pA, vmax, z30, tool2;

MoveJ pB, vmax, z30, tool2;

Chương7 Lập trình dieu khien PDF

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Chương7 Lập trình dieu khien PDF

Uploaded by

Copyright:

Available Formats

Chương 7.

LẬP TRÌNH ĐIỀU KHIỂN

7.1. Khái quát ngôn ngữ lập trình.

Phần mềm điều khiển MÁY TÍNH

Bé nhí M¸y in Bµn phÝm Video Bµn

camera Màn hình

C¶m biÕn §éng C¶m biÕn

6/9/2020 Ky thuat RB K59 ĐK-TĐH (20181) 3

Hai phương pháp lập trình :

Ngôn ngữ lập trình : 3 nhóm :

 Nhóm 1 : Ngôn ngữ lập trình robot : WARE , VAL, AML,….

Ngôn ngữ lập trình : 3 nhóm :

Thế hệ ngôn ngữ lập trình :

Thế hệ ngôn ngữ lập trình :

+ Phần mềm gồm 3 phần tương ứng 3 mức.

7.2. Cấu trúc phần mềm

Ma trận phép biến đổi đối tượng : zA

Ngôn ngữ phần mềm gồm :

7.2. Cấu trúc phần mềm

VÍ DỤ CẤU TRÚC DỮ LIỆU CƠ BẢN

1. Đại lượng vô 3. Phép quay 5. Phép biến đổi

Lập trình hướng đối tượng robot :

Lập trình hướng đối tượng nhiệm vụ :

Các bước xây dựng chương trình :

3. Mô tả công việc bằng trình tự các hành động

1. Giá trị Số : num

Ví dụ 1 : VAR orient orient1;

Tính được : q1=0,707

Tính được : q1=0,965926

5. Biểu diễn vị trí đích : robtarget

5. Biểu diễn vị trí đích : robtarget

Confdata : Dữ liệu cấu hình robot

Confdata : Dữ liệu cấu hình

VÍ DỤ VAR confdata conf15 := [1, -1, 0]

extjoint : Vị trí của các khớp bên ngoài :

VÍ DỤ : VAR extjoint axpos10 := [ 11, 12.3, 9E9, 9E9, 9E9, 9E9]

VÍ DỤ : CONST jointtarget calib_pos := [ [ 0, 0, 0, 0, 0, 0], [ 0, 9E9, 9E9, 9E9,

VÍ DỤ : VAR speeddata vmedium := [ 1000, 30, 200, 15 ];

(NGÔN NGỮ RAPID)

Các phép biến đổi

CalcRobT ( Joint_target Tool [\WObj] )

VÍ DỤ : VAR robtarget p1;

CalcJointT ( [\UseCurWObjPos] Rob_target Tool [\WObj] )

VÍ DỤ : VAR jointtarget jointpos2;

VAR pose pose1;

PoseMult (Pose Multiply) : Nhân hai ma trận PBD

VAR pose pose1;

VAR pose pose1;

VAR num anglex;

VÍ DỤ : 1. MoveJ p1, vmax, z30, tool2;

VÍ DỤ : 1. MoveJSync p1, vmax, z30, tool2, "proc1";

MoveJSync ToPoint [\ID] Speed [\T] Zone Tool [\WObj] ProcName

7. MoveAbsJ : Hàm di chuyển với vị trí khớp tuyệt đối

StopMove [\Quick] [\AllMotionTasks]

Viết chương trình mô tả sự hoạt động của robot sau

1. Viết ma trận đồng

Vecto mô trí của TCP (Tool Center Point(OT) là :

Vị trí gốc các KTĐ đối tượng : OG [150, 50, 0]T

Độ cao của chi tiết : 50

Vị trí điểm E : [200,100,150]T

KTĐ của TCP ở vị trí ban đầu :

KTĐ của TCP ở vị trí B :

// Khai báo các phép biến đổi - cấu trúc dữ liệu