tổng quan về hệ truyền động trong máy cnc. mô phỏng nội suy trên matlab, điều khiển vẽ trên simulink

Chương 1. Tổng quan về hệ truyền động trong máy CNC
Hệ truyền động điện trong máy CNC sẽ đảm nhận các chuyển động theo từng
trục. Tín hiệu điều khiển cho hệ truyền động lấy từ bộ nội suy đóng vai trò là tín
hiệu đặt.
Hình 1.1 Cấu trúc hệ truyền động sử dụng động cơ bước
Các máy CNC hiện nay không áp dụng loại điều khiển trên nữa vì trên thực
tế tồn tại nhiều đại lượng ảnh hưởng tới lực cản trong quá trình chạy dao như độ
bền khác nhau của vật liệu, trạng thái khác nhau của dao cụ. Hiện nay các
CNC chỉ áp dụng hệ truyền động điện điều khiển theo mạch kín có phản hồi vị
trí. Để cải thiện cho đặc tính động học của hệ thống còn đưa vào mạch vòng
điều chỉnh tốc độ và dòng điện bên trong mạch vòng vị trí.
Hình 1.2 Sơ đồ cấu trúc hệ truyền động điện 1 trục
1
Mạch điều chỉnh phải hoạt động sao cho sai lệch giữa vị trí đặt và vị trí thực là
nhỏ nhất và không chịu ảnh hưởng của các nhiễu loạn trên đường dịch chuyển,
muốn vậy hệ truyền động điện vị trí theo từng trục phải thoả mãn những điều
kiện sau:
+Hệ số khuếch đại tốc độ phải lớn.Các hệ số khuếch đại từng trục
phải bằng nhau để tránh dập máy.
+Phải có độ giảm chấn cao tránh hiện tượng dao động.
+Hằng số thời gian của hệ phải nhỏ,các hằng số thời gian cơ khí của
bộ truyền động.
+Độ bền vật liệu phải cao.
2
Chương 2. Tổng hợp bộ điều khiển
2.1. Cấu trúc chung của hệ điều khiển
Cấu trúc chung của hệ điều khiển vị trí gồm ba mạch vòng từ trong ra ngoài
là: mạch vòng dòng điện, mạch vòng tốc độ và mạch vòng vị trí .
Hình 2.1 Sơ đồ cấu trúc chung của hệ điều chỉnh vị trí sử dụng hệ chấp
hành T-Đ
Trong đó:

R
I
: Bộ điều khiển dòng điện.
R
ω
: Bộ điều khiển tốc độ.
R
ϕ
: Bộ điều khiển vị trí.
ĐC: Động cơ điện một chiều kích từ độc lập.
K
i
, K
ω
, K
ϕ
: Các hệ số khuếch đại.
T
i
, T
ω
, T
ϕ
: Các hằng số thời gian.
FT: Máy phát tốc.
Cấu trúc hệ điều khiển vị trí và phương pháp tổng hợp các mạch vòng
Trong hệ điều chỉnh vị trí có ba mạch vòng: mạch vòng dòng điện, mạch
vòng tốc độ, mạch vòng vị trí. Hệ thống truyền động này bắt buộc phải đảo
chiều được.
3

Hình 2.2 là sơ đồ cấu trúc hệ điều khiển vị trí sử dụng động cơ điện một
chiều.
Quan hệ giữa ω và ϕ :
0
t
o
dt
φ φ ω
= +
∫

Hình 2.2 Sơ đồ cấu trúc hệ điều khiển vị trí
4
2.2. Tổng hợp mạch vòng dòng điện
Hình 2.3 Sơ đồ mạch vòng điều chỉnh dòng điện
Trong đó:
u
u
u
L
R
T =
: Hằng số thời gian điện từ của mạch phần ứng
K
i
=R
s
: Điện trở của mạch Sensor
T
i

=R.C : Hằng số thời gian của sensor dòng điện.
J : Momen quán tính
Viết ngắn gọn lại ta có sơ đồ :
Hình 2.4 Mạch vòng dòng điện
Từ sơ đồ trên hình 2.3 và hình 2.4 ta có hàm truyền của đối tượng điều
khiển của mạch vòng điều chỉnh dòng điện:
.
( )
( ) (2.1)
( ) (1 )(1 )(1 )(1 )
i
cl
i u
oi
dk dk v i u
K
K
U p R
S p
U p T P T P T P T P
= =
+ + + +
Trong đó:
T
dk
≈ 100 µs, T
v
≈2.5 ms, T
i
≈2 ms, T

ư
≈100 ms.
Thay T
si
= T
dk
+ T
v
+ T
i
<< T
ư
bỏ qua các hệ số bậc cao ta có:
.
( ) (2.2)
(1 )(1 )
i
cl
u
oi
si u
K
K
R
S P
T P T P
=
+ +
5
Áp dụng tiêu chuẩn tối ưu modul ta có hàm truyền hệ thống kín:

2 2
1
(2.3)
1 2 2
OMi
F
p p
δ δ
τ τ
=
+ +
Mặt khác trên hình 2.4 ta có:
( ).
( ) (2.4)
1 ( ).
i oi
OMi
i oi
R p S
F p
R p S
=
+
))(1(
)(
).(
)(
)(
pFS
pF

SpFS
pF
pR
OMioi
OMi
oiOMioi
OMi
i
−
=
−
=








++
++
++
=
22
22
221
1
)1)(1(
/.

221
1
)(
pp
PTPT
RKK
pp
pR
usi
uicl
i
δδ
δδ
ττ
ττ
)1(2.
)1)(1(
1
)(
pp
PTPT
R
K
K
pR
usi
u
i
cl
i

δδ
ττ
+
++
=
Chọn τ
δ
= min(T
si
, T
ư
) = T
si
Vậy ta có hàm truyền của bộ điều chỉnh dòng điện như sau:
1 .
1
( ) 1 (2.5)
2. . .
2 . . .
u u u
i
si
cl i si u
cl i
u
T P R T
R p
T
K K T T P
p K K

R
 
+
= = +
 ÷
 
R
i
(p) là khâu tỷ lệ - tích phân PI
Vậy sơ đồ cấu trúc của hệ điều chỉnh vị trí còn lại như trên hình 2.5, trong
đó ta lấy hàm truyền đạt của mạch vòng dòng điện là khâu quán tính bậc nhất,
bỏ qua các vô cùng bé bậc cao.
Hình 2.5 Sơ đồ cấu trúc của hệ điều chỉnh vị trí
6
2.3. Tổng hợp mạch vòng tốc độ
Viết gọn sơ đồ hình 2.5 ta có sơ đồ mạch vòng điều chỉnh tốc độ như trên
hình 2.6
Hình 2.6 Sơ đồ mạch vòng điều chỉnh tốc độ
Hàm truyền hệ hở:
.
(2.6)
. . . (1 . )
w
ow
i c sw
R K
S
K CuT p pT
=
+

Với T
s
ω
=T
ω
+ 2.T
si
→ T
s
ω
rất nhỏ.
Áp dụng tiêu chuẩn tối ưu modul:
2 2
1
(2.7)
1 2 2
OMw
F
p p
δ δ
τ τ
=
+ +
)1(.
)(
ω
OMow
OMw
owOMwow
OMw

w
FS
F
SFS
F
pR
−
=
−
=








++
−
+
++
=
22
22
221
1
1.
)1(
.

221
1
)(
pp
pTpTCuK
KR
pp
pR
swCi
w
w
δδ
δδ
ττ
ττ
)1(2.
)1(
.
1
)(
pp
pTpTCuK
KR
pR
swCi
w
w
δδ
ττ
+

+
=
Chọn
sw
T=
δ
τ
Ta có:
. .
( ) (2.8)
. .2
i C
w
w sw
K Cu T
R p
R K T
=
Vậy R
ω
(p) là khâu tỷ lệ P
Tiêu chuẩn này được sử dụng khi hệ thống khởi động đã mang tải, lúc đó
ta không coi I
C
là nhiễu nữa.
Áp dụng tiêu chuẩn tối ưu đối xứng:
7
2 2 3 3
1 4
(2.9)

1 4 8 8
ODw
p
F
p p p
δ
δ δ δ
τ
τ τ τ
+
=
+ + +
)1(.
)(
ωω
ω
ωωω
ω
ω
OMo
OM
oOMo
OM
FS
F
SFS
F
pR
−
=

−
=






+++
+
−
+
+++
+
=
3322
3322
8841
41
1
)1(
.
8841
41
)(
ppp
p
pTpTCuK
KR
ppp

p
pR
sCi
δδδ
δ
ω
ω
δδδ
δ
ω
τττ
τ
τττ
τ
)1(.8.
)1(
.
41
)(
22
pp
pTpTCuK
KR
p
pR
sCi
δδ
ω
ω
δ

ω
ττ
τ
+
+
+
=
Ta chọn τ
σ
= T
s
ω
ta có:
2
1 4
( ) (2.10)
.
.8
. .
s
i C
p
R p
R K
T p
K CuT
δ
ω
ω
ω

τ
+
=
Vậy R
ω
(p) là khâu tỷ lệ - tích phân PI
Đó là khâu vô sai cấp hai đối với đại lượng đặt là vô sai cấp một đối với
đại lượng nhiễu I
C
.

2.4. Tổng hợp mạch vòng vị trí
Sơ đồ cấu trúc hệ thống điều chỉnh vị trí còn lại như sau:
Hình 2.7 Sơ đồ cấu trúc hệ thống điều chỉnh vị trí
Trong đó:
8
i
K
r
1
=
: Hệ số khuếch đại của bộ truyền lực. (2.11)
T
s
ω

= T
ω
+ 2T
si

= T
ω
+ 2(T
dk
+ T
v
+ T
i
). (2.12)
Tổng hợp mạch vòng vị trí cũng tương tự như tổng hợp mạch vòng tốc độ,
ta dùng tiêu chuẩn tối ưu modul và hàm truyền đạt của mạch vòng tốc độ lấy là
khâu bậc hai, bỏ qua các khâu bậc cao.
Vậy:
( ) 1 1
. (2.13)
( ) 1 2
d s
p
p T p K
ω ω
ω
ω
=
+
Khi dùng chuẩn tối ưu module.
( ) 1 1
. (2.14)
( ) 1 4
d s
p

p T p K
ω ω
ω
ω
=
+
Khi dùng chuẩn tối ưu đối xứng.
Viết gọn sơ đồ hình 2.7 ta có sơ đồ mạch vòng điều chỉnh vị trí như trên
hình 2.8
Hình 2.8 Sơ đồ mạch vòng điều chỉnh vị trí
Nếu khi tổng hợp mạch vòng vị trí R
ϕ
(p) dùng chuẩn tối ưu modul, ta có
hàm truyền của đối tượng như sau:
.
( ) (2.15)
(1 2 ) (1 )
r
o
s
K
K
K
S p
T p p T p
ϕ
ω
ϕ
ω ϕ
=

+ +
Áp dụng tiêu chuẩn tối ưu modul:
pT
p
p
pTpK
KK
pR
s
r
ϕ
σ
σ
ωω
ϕ
ϕ
τ
τ
+
+
+
=
1
)1(
.2.
)21.(.
.
1
)(
Chọn τ

σ
= T
ϕ
ta có:
( ) (1 2 ) (2.16)
. .2
s
r
K
R p T p
K K T
ω
ϕ ω
ϕ ϕ
= +
( ) (2.17)
D
R p K K p
ϕ ϕ
= +
Với :
ϕϕ
ω
ϕ
TKK
K
K
r
2
=

9
ϕϕ
ωω
TKK
TK
K
r
s
D
2
=
R
ϕ
(p) là khâu tỷ lệ - đạo hàm PD
Khi tổng hợp mạch vòng vị trí dùng chuẩn tối ưu đối xứng ta có hàm
truyền của đối tượng:
.
( ) (2.18)
(1 4 ) (1 )
r
o
s
K
K
K
S p
T p p T p
φ
ω
ϕ

ω φ
=
+ +
Áp dụng tiêu chuẩn tối ưu đối xứng ta có:
pT
p
p
pTpK
KK
pR
s
r
ϕ
σ
σ
ωω
ϕ
ϕ
τ
τ
+
+
+
=
1
1
2.
)41(.
.
1

)(
Ta chọn τ
σ
= T
ϕ
có:
( ) (1 4 ) (2.19)
. .2
s
r
K
R p T p
K K T
ω
ϕ ω
φ φ
= +
R
ϕ
(p) cũng là khâu tỷ lệ – đạo hàm PD
2.5. Áp dụng cho động cơ 1 chiều kích từ độc lập có thông số sau:
P
đm
Công suất định mức của động cơ 1,5 KW
U
đm
Điện áp định mức 220 V
N
đm
Tốc độ quay định mức 1500 V/ph

η
đm
Hiệu suất danh định của động cơ 90 %
L Điện cảm phần ứng 0,2 H
T
i
Hằng số thời gian máy biến dòng 0,002 s
T
v
Hằng số thời gian bộ chỉnh lưu 0,002
5
s
T
đk
Hằng số thời gian mạch điều khiển bộ chỉnh lưu 0,000
1
s
T
ω
Hằng số thời gian máy phát tốc
0,001 s
T
ϕ
Hằng số thời gian bộ cảm biến vị trí 0,3 s
10
l Chiều dài quãng đường cần di chuyển 100 Cm
R Bán kính trục truyền tải cuối cùng 0,32 Cm
Bảng 2.1 Các thông số của động cơ
Các phương trình phản ứng phần ứng trong động cơ điện một chiều:
U

ư
= E
ư
+ (R
ư
+ R
f
).I
ư
. (2.20)
Trong đó:
U
ư
: Điện áp phần ứng.
R
f
: Điện trở phụ trong mạch phần ứng.
I
ư
: Dòng điện mạch phần ứng.
R
ư
: Điện trở mạch phần ứng.
R
ư
= r
ư
+ r
cf
+ r

b
+ r
ct
(2.21)
r
ư
: Điện trở cuộn dây phần ứng.
r
cf
: Điện trở cực từ phụ.
r
b
: Điện trở cuộn bù.
r
ct
: Điện trở tiếp xúc chổi điện.
E
ư
: Suất điện động phần ứng.
ωωω
Φ
Π
=Φ==
a
PN
K
2
Cu. E
u
(2.22)

Trong đó:
P: Số đôi cực từ chính.
N: Số thanh dẫn tác dụng của cuộn dây phần ứng.
Φ: Từ thông kích từ dưới một cực.
ω: Tốc độ góc.
a
PN
K
Π
=
2
: Hệ số cấu tạo của động cơ.
E
ư
= K
e
Φn
n: Tốc độ rôto.
2 .
(2.23)
60 9,55
n n
π
ω
= =
1500
157 ( / ) (2.24)
9,55
dm
rad s

ω
= =
0,105 (2.25)
9,55
e
K
K K= =
Phương trình đặc tính cơ điện:
(2.26)
u f u f
u u
u u
R R R R
U U
I I
K K Cu Cu
ω
+ +
= − = −
Φ Φ
11
Phương trình đặc tính cơ:
2 2
(2.27)
( )
u f u f
u u
R R R R
U U
M M

K Cu
K Cu
ω
+ +
= − = −
Φ
Φ
Trong đó:

Cu
M
I
dt
u
=
M
đt
= M
cơ
= M
Tính mômen định mức:
P
đm
= M
đm
. ω
đm
1500
9,55 (2.28)
157

dm
dm
dm
P
M Nm
ω
⇒ = = =
1500
6,82 (2.29)
220
dm
dm
dm
P
I A
U
⇒ = = =
9,55
1,4 (2.30)
6,82
dm
dm
M
Cu
I
⇒ = = =
Tính gần đúng R
ư
theo công thức:
220

0,5(1 ). 0,5(1 0,9). 1,6 (2.31)
6,82
dm
u dm
dm
U
R
I
η
= − = − = Ω
L = L
b
+ L
k
+ L
ưd
= 0,2 H
0,2
0,125 (2.32)
1,6
u
u
L
T s
R
⇒ = = =
Từ sơ đồ cấu trúc hệ điều khiển vị trí trên hình 2.5 ta có:
U
đ
= K

cl
.U
đk
Chọn U
đk
= 10 V ta có:
220
22 (2.33)
10
dm
cl
dk
U
K
U
= = =
Hàm truyền của bộ điều chỉnh dòng điện:
1
1 (2.34)
2. . .
u u
i
cl i si u
R T
R
K K T T p
 
= +
 ÷
 

T
si
= T
i
+ T
v
+ T
đk
= 2 ms + 2,5 ms + 0,1 ms = 4,6.10
–3
s
T
ư
= 0,125 s
12
R
ư
= 1,6 Ω
K
cl
= 22
Chọn U
iđ
= 7 V
02,1
82,6
7
===
dm
id

i
I
U
K








+=⇒
−
p
pR
i
125,0
1
1
10.6,4.02,1.22.2
125,0.6,1
)(
3
1
( ) 0,968 1 (2.35)
0,125
i
R p
p

 
⇒ = +
 ÷
 
Cũng trên hình 2.5 ta có:
U
ω
đ
= ω.K
ω
Chọn U
ω
đ
= 4,7 V
4,7
0,03
157
K
ω
⇒ = =
Ta có:
T
s
ω
=T
ω
+ 2.T
si
Với T
ω

= 1 ms, T
si
= 4,6.10
–3
s
C
2 2
2,45.1,6
T 2
1,4
JR
s
Cu
= = =

⇒ T
s
ω
= 10
-3
+2.4,6.10
-3
= 1.10
-3
+9,2.10
-3
= 10,2.10
-3
s
1

3
. .
1,02.1,4.2
( ) 2916 (2.36)
. .2
1,6.0,03.2.10,2.10
C
s
K Cu T
R p
R K T
ω
ω ω
−
= = =
1,6
0,5714
. 1,4.2
C
R
Cu T
= =
Ta có:
50
032,0 2
100
2
2. ===⇒==
ππ
πϕ

r
l
nrnrl
(vòng)
)(100 2 radn
r
l
ππϕ
===⇒
13
Trên hình 2.5 tín hiệu xác định vị trí đặt phải xấp xỉ tín hiệu phản hồi tức
là: ϕ.K
ϕ
= U
ϕ
đ
10
0,032
100
K
ϕ
π
⇒ = =
1
0
. . . .Kr dt Kr t
φ ω ω
= =
∫
K

r
: Hệ số khuếch đại của bộ truyền lực.
1
2.157
314
.
===
t
Kr
ω
ϕ
Bộ điều chỉnh vị trí R
ϕ
(p)
)02,01(
3.0.2.032,0.1
03,0
)21(
2
)( ppT
TKK
K
pR
s
r
+=+=
ω
ϕϕ
ω
ϕ

( ) 1,5625(1 0,02 ) (2.37)R p p
φ
= +
Ta có sơ đồ mô phỏng sau:
Hình 2.9 Sơ đồ mô phỏng trên simulink
14
Kết quả thu được khi tín hiệu vào là định mức U
ϕ
đặt
= 10V, I
tải
= 6A
a) Vị trí
Hình 2.10: Đồ thị mô phỏng khi U
ϕ
đặt
= U
ϕ
đặtđm
= 10 V
Thời gian quá độ là T
max
= 2,6s
Độ quá chỉnh là σ
max
= 22,5
Số lần dao động n = 1.
Góc quay của rotor khi hệ thống ổn định là 312,5 rad
b) Vận tốc
Hình 2.11 Đồ thị mô phỏng tốc độ của động cơ

Thời gian quá độ là T
max
= 3,2s
Độ quá chỉnh là σ
max
= 515
Số lần dao động n = 1
Tốc độ của động cơ khi hệ ở trạng thái ổn định là ω = -0,0002 rad/s
15
c) Dòng điện
Hình 2.12 Đặc tính dòng điện của động cơ
Thời gian quá độ là T
max
= 2,2s
Độ quá chỉnh là σ
max
= 1,95.10
4
Số lần dao động n = 1
Dòng điện của động cơ khi hệ ở trạng thái ổn định là I = 9,58 A
16
Chương 3. Chương trình nội suy
3.1. Phương pháp nội suy
Sử dụng phương pháp nội suy theo hàm đánh giá
a. Nội suy theo đường thẳng
∆
X=X
B
-X
A

: kích thước dịch chuyển theo trục X
∆
Y=Y
B
-Y
A
: kích thước dịch chuyển theo trục Y
dX: Bước dịch chuyển theo trục X
dY: Bước dịch chuyển theo trục Y
Phương trình toán học:

A A
B A B A
Y Y X X
Y Y X X
− −
=
− −

( ) ( ) 0
A A
Y Y X X X Y− ∆ − − ∆ =
Từ phương trình này ta đưa ra công thức của hàm đánh giá đường thẳng như
sau:
( ) ( )
A A
F Y Y X X X Y= − ∆ − − ∆
Với (X,Y) bất kỳ =>Có 3 khả năng xảy ra:
+ Nếu F =0 =>(X,Y) nằm trên đường thẳng
+ Nếu F >0 (X,Y) nằm phía trên đường thẳng nếu

∆
X > 0
(X,Y) nằm phía dưới đường thẳng nếu
∆
X < 0
+ Nếu F <0 (X,Y) nằm phia dưới đường thẳng nếu
∆
X > 0
(Y,Y) nằm phía trên đường thẳng nếu
∆
X < 0
* Ta có lưu đồ thuật toán:
17
Hình 3.1 Thuật toán nội suy đường thẳng
b. Nội suy đường tròn
Giả sử phải tính các điểm nội suy trên cung AB thuận chiều kim đồng hồ
Phương trình:
2 2 2
( ) ( )
o o
X X Y Y R− + − =
Trong đó :
2 2
( ) ( )
o o
R X X Y Y= − + −
Với 1 điểm có toạ độ (X,Y) bất kỳ trên mặt phẳng người ta tính hàm đánh giá
của nó như sau:
2 2 2
( ) ( )

o o
F X X Y Y R= − + − −
có 3 khả năng xảy ra:
+ F=0 =>(X,Y) thuộc đường tròn
+ F>0 =>(X,Y) ngoài đường tròn
+ F<0 =>(X,Y) nằm trong đường tròn
Với Ai(Xi,Yi) ta có thuật toán tính điểm nội suy tiếp theo như sau:
+ Tính hàm đánh giá Fi của Ai theo công thức

2 2 2
( ) ( )
i i o i o
F X X Y Y R= − + − −
So sánh Fi với 0
18
* Ta có lưu đồ thuật toán
Hình 3.2 Thuật toán nội suy đường tròn
19
3.2. Chương trình viết trong M_File
Hình 3.3 Hình cần nội suy
%Nhap cac toa do diem
xa1=1;ya1=2;
xa2=1;ya2=3;
xaa=2;yaa=4;
xa3=1;ya3=5;
xa4=1;ya4=6;
xa5=2;ya5=7;
xa6=3;ya6=7;
xab=4;yab=6;
xa7=5;ya7=7;

xa8=6;ya8=7;
xa9=7;ya9=6;
xa10=7;ya10=5;
xac=6;yac=4;
xa11=7;ya11=3;
xa12=7;ya12=2;
xa13=6;ya13=1;
xa14=5;ya14=1;
xad=4;yad=2;
xa15=3;ya15=1;
xa16=2;ya16=1;
20
%Toa do tam cua cac duong tron
xo1=1;yo1=4;r1=1;
xo2=1;yo2=7;r2=1;
xo3=4;yo3=7;r3=1;
xo4=7;yo4=7;r4=1;
xo5=7;yo5=4;r5=1;
xo6=7;yo6=1;r6=1;
xo7=4;yo7=1;r7=1;
xo8=1;yo8=1;r8=1;
x=[];y=[];t=[];
x(1)=0;y(1)=0;t(1)=0;
dx=0.01;dy=0.01;dt=0.01;
k=1;
%Noi suy tu O den A1
while (x(k)<xa1|y(k)<ya1)
F =(y(k)-0)*(xa1-0)-(x(k)-0)*(ya1-0);
if (F>=0)
x(k+1)=x(k)+dx;

y(k+1)=y(k);
t(k+1)=t(k)+dt;
else
x(k+1)=x(k);
y(k+1)=y(k)+dy;
t(k+1)=t(k)+dt;
end
k=k+1;
end
%Noi suy tu A1 den A2
dx=0;dy=0.01;
while (y(k)<ya2)
x(k+1)=x(k);
y(k+1)=y(k)+dy;
t(k+1)=t(k)+dt;
k=k+1;
end
%Noi suy tu A2 den Aa
dx=0.01;dy=0.01;
21
while (x(k)<xaa|y(k)<yaa)
F=(x(k)-xo1)^2+(y(k)-yo1)^2-r1*r1
if (F>=0)
x(k+1)=x(k);
y(k+1)=y(k)+dy;
t(k+1)=t(k)+dt;
else
x(k+1)=x(k)+dx;
y(k+1)=y(k)
t(k+1)=t(k)+dt;

end
k=k+1;
end
%Noi suy tu Aa den A3
dx=-0.01;dy=0.01;
while (x(k)>xa3|y(k)<ya3)
F=(x(k)-xo1)^2+(y(k)-yo1)^2-r1*r1
if (F>=0)
x(k+1)=x(k)+dx;
y(k+1)=y(k);
t(k+1)=t(k)+dt;
else
x(k+1)=x(k);
y(k+1)=y(k)+dy;
t(k+1)=t(k)+dt;
end
k=k+1;
end
%Noi suy tu A3 den A4
dx=0;dy=0.01;
while (y(k)<ya4)
x(k+1)=x(k);
y(k+1)=y(k)+dy;
t(k+1)=t(k)+dt;
k=k+1;
end
%Noi suy tu A4 den A5
22
dx=0.01;dy=0.01;
while (x(k)<xa5|y(k)<ya5)

F=(x(k)-xo2)^2+(y(k)-yo2)^2-r2*r2
if (F>=0)
x(k+1)=x(k);
y(k+1)=y(k)+dy;
t(k+1)=t(k)+dt;
else
x(k+1)=x(k)+dx;
y(k+1)=y(k)
t(k+1)=t(k)+dt;
end
k=k+1;
end
%Noi suy tu A5 den A6
dx=0.01;dy=0;
while (x(k)<xa6)
x(k+1)=x(k)+dx;
y(k+1)=y(k);
t(k+1)=t(k)+dt;
k=k+1;
end
%Noi suy tu A6 den Ab
dx=0.01;dy=-0.01;
while (x(k)<xab|y(k)>yab)
F=(x(k)-xo3)^2+(y(k)-yo3)^2-r3*r3
if (F>=0)
x(k+1)=x(k)+dx;
y(k+1)=y(k);
t(k+1)=t(k)+dt;
else
x(k+1)=x(k);

y(k+1)=y(k)+dy;
t(k+1)=t(k)+dt;
end
k=k+1;
end
23
%Noi suy tu Ab den A7
dx=0.01;dy=0.01;
while (x(k)<xa7|y(k)<ya7)
F=(x(k)-xo3)^2+(y(k)-yo3)^2-r3*r3
if (F>=0)
x(k+1)=x(k);
y(k+1)=y(k)+dy;
t(k+1)=t(k)+dt;
else
x(k+1)=x(k)+dx;
y(k+1)=y(k);
t(k+1)=t(k)+dt;
end
k=k+1;
end
%Noi suy tu A7 den A8
dx=0.01;dy=0;
while (x(k)<xa8)
x(k+1)=x(k)+dx;
y(k+1)=y(k);
t(k+1)=t(k)+dt;
k=k+1;
end
%Noi suy tu A8 den A9

dx=0.01;dy=-0.01;
while (x(k)<xa9|y(k)>ya9)
F=(x(k)-xo4)^2+(y(k)-yo4)^2-r4*r4
if (F>=0)
x(k+1)=x(k)+dx;
y(k+1)=y(k);
t(k+1)=t(k)+dt;
else
x(k+1)=x(k);
y(k+1)=y(k)+dy;
t(k+1)=t(k)+dt;
end
k=k+1;
24
end
%Noi suy tu A9 den A10
dx=0;dy=-0.01;
while (y(k)>ya10)
x(k+1)=x(k);
y(k+1)=y(k)+dy;
t(k+1)=t(k)+dt;
k=k+1;
end
%Noi suy tu A10 den Ac
dx=-0.01;dy=-0.01;
while (x(k)>xac|y(k)>yac)
F=(x(k)-xo5)^2+(y(k)-yo5)^2-r5*r5
if (F>=0)
x(k+1)=x(k);
y(k+1)=y(k)+dy;

t(k+1)=t(k)+dt;
else
x(k+1)=x(k)+dx;
y(k+1)=y(k);
t(k+1)=t(k)+dt;
end
k=k+1;
end
%Noi suy tu Ac den A11
dx=0.01;dy=-0.01;
while (x(k)<xa11|y(k)>ya11)
F=(x(k)-xo5)^2+(y(k)-yo5)^2-r5*r5
if (F>=0)
x(k+1)=x(k)+dx;
y(k+1)=y(k);
t(k+1)=t(k)+dt;
else
x(k+1)=x(k);
y(k+1)=y(k)+dy;
t(k+1)=t(k)+dt;
end
25