Bài tập lớn môn hệ thống điều khiển máy CNC (hust) có file mô phỏng

BÁO CÁO BÀI TẬP LỚN HỆ THỐNG ĐIỀU KHIỂN MÁY CNC Giảng viên hướng dẫn: TS. Dương Minh Đức Nhóm 18 Trương Hữu Đô 20181392 Trần Văn Long 20181629 Nguyễn Đắc Quang 20174132 Hà Nội, tháng 7 năm 2022   MỤC LỤC BÁO CÁO BÀI TẬP LỚN HỆ THỐNG ĐIỀU KHIỂN MÁY CNC Giảng viên hướng dẫn: TS. Dương Minh Đức Nhóm 18 Trương Hữu Đô 20181392 Trần Văn Long 20181629 Nguyễn Đắc Quang 20174132 Hà Nội, tháng 7 năm 2022   MỤC LỤC BÁO CÁO BÀI TẬP LỚN HỆ THỐNG ĐIỀU KHIỂN MÁY CNC Giảng viên hướng dẫn: TS. Dương Minh Đức Nhóm 18 Trương Hữu Đô 20181392 Trần Văn Long 20181629 Nguyễn Đắc Quang 20174132 Hà Nội, tháng 7 năm 2022   MỤC LỤC BÁO CÁO BÀI TẬP LỚN HỆ THỐNG ĐIỀU KHIỂN MÁY CNC Giảng viên hướng dẫn: TS. Dương Minh Đức Nhóm 18 Trương Hữu Đô 20181392 Trần Văn Long 20181629 Nguyễn Đắc Quang 20174132 Hà Nội, tháng 7 năm 2022   MỤC LỤC BÁO CÁO BÀI TẬP LỚN HỆ THỐNG ĐIỀU KHIỂN MÁY CNC Giảng viên hướng dẫn: TS. Dương Minh Đức Nhóm 18 Trương Hữu Đô 20181392 Trần Văn Long 20181629 Nguyễn Đắc Quang 20174132 Hà Nội, tháng 7 năm 2022   MỤC LỤC BÁO CÁO BÀI TẬP LỚN HỆ THỐNG ĐIỀU KHIỂN MÁY CNC Giảng viên hướng dẫn: TS. Dương Minh Đức Nhóm 18 Trương Hữu Đô 20181392 Trần Văn Long 20181629 Nguyễn Đắc Quang 20174132 Hà Nội, tháng 7 năm 2022   MỤC LỤC BÁO CÁO BÀI TẬP LỚN HỆ THỐNG ĐIỀU KHIỂN MÁY CNC Giảng viên hướng dẫn: TS. Dương Minh Đức Nhóm 18 Trương Hữu Đô 20181392 Trần Văn Long 20181629 Nguyễn Đắc Quang 20174132 Hà Nội, tháng 7 năm 2022   MỤC LỤC BÁO CÁO BÀI TẬP LỚN HỆ THỐNG ĐIỀU KHIỂN MÁY CNC Giảng viên hướng dẫn: TS. Dương Minh Đức Nhóm 18 Trương Hữu Đô 20181392 Trần Văn Long 20181629 Nguyễn Đắc Quang 20174132 Hà Nội, tháng 7 năm 2022   MỤC LỤC BÁO CÁO BÀI TẬP LỚN HỆ THỐNG ĐIỀU KHIỂN MÁY CNC Giảng viên hướng dẫn: TS. Dương Minh Đức Nhóm 18 Trương Hữu Đô 20181392 Trần Văn Long 20181629 Nguyễn Đắc Quang 20174132 Hà Nội, tháng 7 năm 2022   MỤC LỤC BÁO CÁO BÀI TẬP LỚN HỆ THỐNG ĐIỀU KHIỂN MÁY CNC Giảng viên hướng dẫn: TS. Dương Minh Đức Nhóm 18 Trương Hữu Đô 20181392 Trần Văn Long 20181629 Nguyễn Đắc Quang 20174132 Hà Nội, tháng 7 năm 2022   MỤC LỤC BÁO CÁO BÀI TẬP LỚN HỆ THỐNG ĐIỀU KHIỂN MÁY CNC Giảng viên hướng dẫn: TS. Dương Minh Đức Nhóm 18 Trương Hữu Đô 20181392 Trần Văn Long 20181629 Nguyễn Đắc Quang 20174132 Hà Nội, tháng 7 năm 2022   MỤC LỤC BÁO CÁO BÀI TẬP LỚN HỆ THỐNG ĐIỀU KHIỂN MÁY CNC Giảng viên hướng dẫn: TS. Dương Minh Đức Nhóm 18 Trương Hữu Đô 20181392 Trần Văn Long 20181629 Nguyễn Đắc Quang 20174132 Hà Nội, tháng 7 năm 2022   MỤC LỤC BÁO CÁO BÀI TẬP LỚN HỆ THỐNG ĐIỀU KHIỂN MÁY CNC Giảng viên hướng dẫn: TS. Dương Minh Đức Nhóm 18 Trương Hữu Đô 20181392 Trần Văn Long 20181629 Nguyễn Đắc Quang 20174132 Hà Nội, tháng 7 năm 2022   MỤC LỤC BÁO CÁO BÀI TẬP LỚN HỆ THỐNG ĐIỀU KHIỂN MÁY CNC Giảng viên hướng dẫn: TS. Dương Minh Đức Nhóm 18 Trương Hữu Đô 20181392 Trần Văn Long 20181629 Nguyễn Đắc Quang 20174132 Hà Nội, tháng 7 năm 2022   MỤC LỤC BÁO CÁO BÀI TẬP LỚN HỆ THỐNG ĐIỀU KHIỂN MÁY CNC Giảng viên hướng dẫn: TS. Dương Minh Đức Nhóm 18 Trương Hữu Đô 20181392 Trần Văn Long 20181629 Nguyễn Đắc Quang 20174132 Hà Nội, tháng 7 năm 2022   MỤC LỤC BÁO CÁO BÀI TẬP LỚN HỆ THỐNG ĐIỀU KHIỂN MÁY CNC Giảng viên hướng dẫn: TS. Dương Minh Đức Nhóm 18 Trương Hữu Đô 20181392 Trần Văn Long 20181629 Nguyễn Đắc Quang 20174132 Hà Nội, tháng 7 năm 2022   MỤC LỤC dfBÁO CÁO BÀI TẬP LỚN HỆ THỐNG ĐIỀU KHIỂN MÁY CNC Giảng viên hướng dẫn: TS. Dương Minh Đức Nhóm 18 Trương Hữu Đô 20181392 Trần Văn Long 20181629 Nguyễn Đắc Quang 20174132 Hà Nội, tháng 7 năm 2022   MỤC LỤC

VIỆN ĐIỆN - -

BÁO CÁO BÀI TẬP LỚN

HỆ THỐNG ĐIỀU KHIỂN MÁY CNC

Giảng viên hướng dẫn: TS Dương Minh Đức

Nhóm 18

Trương Hữu Đô 20181392 Trần Văn Long 20181629 Nguyễn Đắc Quang 20174132

Hà Nội, tháng 7 năm 2022

MỤC LỤC

DANH MỤC HÌNH VẼ

Hình 1.1 Mô hình điều khiển vị trí 7

Hình 1.2 Mô hình bộ điểu khiển 8 Hình 1.3 Mô hình điều khiển vị trí 2 trục X,Y 8

Hình 1.4 Đáp ứng lượng đặt vị trí của trục X 9

Hình 1.5 Đáp ứng lượng đặt vị trí của trục Y 10

Hình 1.6 U điều khiển của trục X 10

Hình 1.7 U điều khiển của trục Y 11

- Đầu vào dạng Step

II Nội suy đường thẳng và đường tròn

Yêu cầu:

- T nộisuy =5ms

- T đk =1ms

- Tăng tốc giảm tốc trước nội suy( ADCBI)

- Dạng quỹ đạo hình thang( Normal Block)

III Vẽ quỹ đạo hình chữ O 

BÀI LÀM

I Thiết kế bộ điều khiển vị trí cho hai trục X, Y

Hàm truyền của mạch vòng vận tốc tương ứng với mỗi trục chuyển động Ox, Oy như sau :

Ta có cấu trúc mạch vòng điều khiển vị trí như sau:

Hình 1.1 Cấu trúc điều điển vị trí

Đối tượng điều khiển vị trí ta thấy gồm khâu tích phân - quán tính bậc nhất

Trong quá trình thiết kế bộ điều khiển vị trí em đã không sử dụng khâu tích phân I mà chỉ dùng thành phần P và D Do đối tượng đã có khâu tích phân đã làm cho sai lệch tính về 0 nên nếu đưa thêm khâu tích phân là không cần thiết Xác định hệ số Kp như sau: Kp=U đk max

e max =101 =10Tiếp theo ta chỉnh định hệ số Td thỏa mãn nhưng yêu cầu ở trên

Hình 1.2 Cấu trúc bộ điều khiển

Hình 1.3 Mô hình điều khiển vị trí 2 trục X,Y

Sau khi kết hợp kết quả ở trên và quá trình chỉnh định bằng ta, chúng em chọn ra hai bộ PD tương ứng để điều khiển vị trí cho hai động cơ trên trục X,Y.

Hình 1.4 Đáp ứng lượng đặt vị trí của trục X

Hình 1.5 Đáp ứng lượng đặt vị trí của trục Y

Hình 1.6 U điều khiển của trục X

Hình 1.7 U điều khiển của trục Y

II Nội suy đường thẳng và đường tròn

1.Nội suy đường thẳng

Cần di chuyển từ điểm A đến điểm B theo quỹ đạo là đường thẳng

 Yêu cầu: Thực hiện nội suy với kiểu tăng tốc/ giảm tốc dạng hình thang (tăng tốc/ giảm tốc trước nội suy)

Thông số cần sử dụng trong quá trình nội suy:

- Tọa độ điểm bắt đầu: A (200, 200)

- Tọa độ điểm kết thúc: B (500, 600)

- Tốc độ ăn dao: F = 100 (mm/s)

- Gia tốc tăng tốc cho phép: A = 50 (mm/s)

- Gia tốc giảm tốc cho phép: B = 50 (mm/s)

- Chu kỳ nội suy: Tns = 5 (m/s)

- Chu kỳ điều khiển: Tdk = 1 (m/s)

1.1 Các bước tiến hành

 Tính toán thông số cần thiết

- Chiều dài đoạn thẳng: L= √(500−200)2+(600−200)2=500(mm)

- cos φ ,sin φ (φ là góc lệch giữa đoạn thẳng AB và trục Ox)

 cos φ = 500−200500 = 0.6

 sin φ = 600−200500 = 0.8

 Tính toán quá trình tăng tốc, giảm tốc và vận tốc không đổi

Với thông số như trên ta có sẽ kiểm tra dạng quỹ đạo là Normal Block hay ShortBlock :2 A F2 + F 2D2 −L=−300<0 Suy ra quỹ đạo dạng Normal Block Ta có bảng :

Tăng tốc Giảm tốc Vận tốc không đổi

Code Matlab:

%% Thoi gian tang toc, giam toc

Tns=5e-3; % Chu ki noi suy

Tdk=1e-3; % Chu ki dieu khien

kT=5; % Ti so Tns/Tdk

F=100; % Toc do an dao

A=50; % Gia toc tang toc

D=50; % Gia toc giam toc

% Toa do diem A(start) va B(end)

xa=200;ya=200;

xb=500;yb=600;

Lx=xb-xa;

Ly=yb-ya;

Ta=F/A; % Thoi gian tang toc

Td=F/D; % Thoi gian giam toc

Tc=(L-F^2/(2*A)-F^2/(2*D))/F; % Thoi gian chuyen dongdeu

else

F=sqrt(L/(1/(2*A)+1/(2*D)));

Ta=F/A; % Thoi gian tang toc

Td=F/D; % Thoi gian giam toc

Tc=0; % Thoi gian chuyen dong deu

end

Tsum=Ta+Tc+Td% Tong thoi gian chuyen dong

Na=int32(Ta/Tns); % So lan noi suy trong giai doan tangtoc

Nd=int32(Td/Tns); % So lan noi suy trong giai doan chuyendong deu

Nc=int32(Tc/Tns); % So lan noi suy trong giai doan giamtoc

Nsum=Na+Nc+Nd; % Tong so lan noi suy

time_ns=linspace(0,Tsum,Nsum);

time_dk=linspace(0,Tsum,kT*Nsum);

 Nội suy thô:

- Sử dụng phương pháp trích mẫu dữ liệu.

- Quá trình tăng tốc: Chuyển động nhanh dần đều, vận tốc tăng từ 0 đến F, gia tốc

A

 Vận tốc sau mỗi chu kỳ nội suy là: V(i+1)=V(i)+ A T ns

 Quãng đường đi được sau mỗi chu kỳ nội suy là: ∆ L a =V (i).T ns

 Độ dịch chuyển trên trục X: ∆ X (i)=∆ L a cos(φ)

 Độ dịch chuyển trên trục Y: ∆ Y (i)=∆ L a sin(φ)

 Từ đó ta có tọa độ của dao qua mỗi chu kỳ nội suy là :

X (i+1)= X (i)+∆ X (i)

Y (i+1)=Y (i)+∆ Y (i)

- Quá trình tốc độ không đổi: Chuyển động đều với vận tốc F, vận tốc sau mỗi chu

kỳ không đổi Tương tự như trên ta xác định được :

∆ L c =F T ns

X(i+1)=X (i)+∆ L c cos(φ)

Y (i+1)=Y (i)+∆ L c sin(φ)

- Quá trình giảm tốc: Chuyển động chậm dần đều, vận tốc sẽ giảm từ F về 0 với giatốc D Tương tự, ta xác định được :

V (i+1)=V (i)−D T

∆ L d =V (i).T ns

X(i+1)=X (i)+∆ L d .cos(φ)

Y (i+1)=Y (i)+∆ L d sin(φ)

Code Matlab:

%% Noi suy tho

Va=linspace(0,F,Na); % Toc do quang duong tang toc

Vc=ones(1,Nc)*F; % Toc do quang duong toc do khong doiVd=linspace(F,0,Nd); % Toc do quang duong giam toc

V=[Va Vc Vd]; % Toc do tren toan quy dao

VX=V*cosphi; % Toc do ung voi truc X

VY=V*sinphi; % Toc do ung voi truc Y

dL=V*Tns; % Quang duong di duoc sau moi Tns

dX=dL*cosphi; % Quang duong di duoc theo truc X

dY=dL*sinphi; % Quang duong di duoc theo truc Y

X_tho = xa; Y_tho = ya;

ylabel('Truc Y(mm)')

title('Quy dao sau khi noi suy tho')

legend({'Acc','Cons','Dec'});

grid on

Kết quả mô phỏng:

 Nội suy tinh:

- Sau quá trình nội suy thô, ta thu được các giá trị sai lệch giữa thời điểm i

và i+1 theo 2 trục là 2 ma trận dX và dY

- Gọi tổng số điểm nội suy là: N S =N A +N C +N D

- Do chu kì điều khiển T đ k nhỏ hơn chu kì nội suy T ns 5 lần nên sau khi nội

suy tinh, kích thước của 2 ma trận mới d X new, d Y new sẽ tăng gấp 5 lần so

với 2 ma trận cũ dX, dY

- Ở đây ta sẽ sử dụng phương pháp tuyến tính:

d X new(j)= dX(j)

5 ,i ≤ j<i+5

- Sau khi nội suy tinh, vị trí 2 trục X, Y sẽ được tính lại theo công thức sau

title('Quy dao Noi suy tinh')

grid on

Kết quả mô phỏng:

 Vẽ đồ thị:

₋ Đồ thị vị trí:

0 1 2 3 4 5 6 7

Time (s) 200

Van toc truc Y

 Sơ đồ mô phỏng trên Simulink:

 Kết quả mô phỏng trên Simulink:

 So sánh:

Cons Dec

Cons Dec

Controller

Cons Dec

Controller

2.Nội suy đường tròn

2.1 Yêu cầu bài toán:

- Thực hiện nội suy với kiểu tăng tốc/ giảm tốc dạng hình thang (tăng tốc/ giảm tốctrước nội suy)

- Tọa độ điểm bắt đầu A(200,0)

- Tọa độ điểm kết thúc B(800,0)

- Tọa độ tâm đường tròn I(500,50)

- Chiều di chuyển theo cung tròn ngắn hay dài (đi theo cung tròn dài)

- Tốc độ ăn dao F = 100 (mm/s)

- Gia tốc tăng tốc cho phép A = 50 (mm/𝑠2)

- Gia tốc giảm tốc cho phép D = 50 (mm/𝑠2)

- Chu kỳ nội suy T ns =5(ms)

- Chu kỳ điều khiển T dk=1 ¿)

1.2 Các bước thực hiện

a.Tính toán

- Các thông số quỹ đạo:

+ R=√(xA−xI )2+( yA− yI )2: bán kính quỹ đạo

- Kiểm tra dạng quỹ đạo là Normal block hay Short block :

F 12

2 A + 2D F2 ≥ L

- Tính toán thời gian tăng tốc (TA), giảm tốc (TD), vận tốc không đổi (TC) :

Hình 2.2.1 Profile quỹ đạo vận tốc góc

b Nội suy thô

Sử dụng phương pháp trích mẫu dữ liệu

Từ giá trị vận tốc góc 𝜔, ta tính được giá trị góc 𝑑𝛼 là góc giữa 2 điểm nội suy ứng với chu kì nội suy là T ns:

𝑑𝛼(i)= 𝜔(i) T ns

𝛼(i+1)= 𝛼(i) − ¿ 𝑑𝛼(i): đi theo chiều kim đồng hồ 𝛼(i+1)= 𝛼(i) + ¿ 𝑑𝛼(i): đi theo ngược chiều kim đồng hồ

dY(i)=Y(i+1) − ¿ Y(i)

c Nội suy tinh

Sau quá trình n i suy thô, ta thu đ c các giá tr sai l ch gi a th i đi m i và ộ ượ ị ệ ữ ờ ể i+1 theo 2 tr c là dX và dY ụ

Do chu kì đi u khi n ề ể T đk nh h n chu kì n i suy ỏ ơ ộ T ns 5 l n nên sau khi n i suy ầ ộ tinh, kích th c ướ d X new, d Y new sẽ tăng g p 5 l n so v i dX, dY ấ ầ ớ

đây ta sẽ s d ng ph ng pháp tuy n tính:

d X new ( j)= dX ( j)5 ,i ≤ j<i+5

Sau khi nội suy tinh xong, vị trí 2 trục X,Y sẽ được tính như sau:

X(i+1)=X(i)+ d X new (i)

Y(i+1)=Y(i)+ d Y new (i)

Hình 2.2.2 Vị trí theo các trục

Hình 2.2.2 Vận tốc theo các trục

Hình 2.2.2 So sánh kết quả

Hình 2.2.2 Chêch lệch giữa kết quả nội suy và vị trí qua bộ điều khiển

3 Mô phỏng hệ thống di chuyển theo quỹ đạo chữ O

Ta thiết kế quỹ đạo như sau:

- Xuất phát từ điểm A (200, 200).

- Đường thứ nhất: Nội suy đường thẳng từ điểm đầu A (200, 200) đến điểm cuối

B (200, 400)

- Đường thứ hai: Nội suy đường tròn từ điểm đầu B (200, 400) đến điểm cuối C

(400, 400), với tâm I1 (300, 400), theo cung tròn ngắn, chiều thuận kim đồng hồ

- Đường thứ ba: Nội suy đường thẳng từ điểm đầu C (400, 400) đến điểm cuối D

(400, 200)

- Đường thứ thư: Nội suy đường tròn từ điểm đầu D (400, 200) đến điểm cuối A

(200, 200), với tâm I2 (300, 200), theo cung tròn ngắn, chiều thuận kim đồng hồ

Kết quả mô phỏng trên Simulink

Xây dựng thuật toán nội suy đường thẳng từ điểm A (0;0) đến điểm B(30;40)

có :

+ Tốc độ tối đa : V =10( mm s )

+ Gia tốc tăng/giảm tốc : A=D=20( mm

s2 )+ Chu kỳ nội suy : T ns =5(ms)

 Số bước cần thiết cho quá trình tăng/giảm tốc : m= t a

T ns= t d

T ns= 0.50.005=100

Có hàm truyền của bộ lọc số tăng giảm tốc hình thang trên miền z:

P(z) Q(z)¿H L(z)= 1m 1−z −m

1−z−1P(z) là tín hiệu đầu ra bộ lọc số, Q(z) là tín hiệu đầu vào bộ lọc số

 P(z)−z−1P(z)= 1m (Q(z)−z −m Q(z))

Áp dụng phép biến đổi z trong miền thời gian liên tục có :

P(k)−P((k−1))= 1 ¿

Gọi ∆ X0 là bước di chuyển sau quá trình tăng/giảm tốc trên trục X

Áp dụng công thức (*) cho tính toán tăng giảm tốc với lượng đặt ∆ X của trục X

có :

∆ X0(k)=∆ X0(k−1)+ 1m (∆ X(k)−∆ X(k−m))Với trục Y có:

∆ Y0(k)=∆ Y0(k−1)+ 1m (∆ Y(k)−∆ Y(k−m))Với vận tốc:

Hình 4.2 Giá trị đặt theo các trục

Hình 4.3 Vị trí theo các trục

Hình 4.4 So sánh giá trị đặt và kết quả sau nội suy

Hình 4.5 Chênh lêch giá trị đặt và kết quả sau nội suy

 Code mô phỏng

Tns=5e-3; % Chu ki noi suy

F=10; % Toc do an dao

A=20; % Gia toc tang toc

D=20; % Gia toc giam toc

% Toa do diem A(start) va B(end)

4.2Tăng giảm tốc sau nội suy đường tròn

Xây dựng thuật toán nội suy đường tròn từ điểm A (0;0) đến điểm B (10;0) có tâm O (5;0):

+ Tốc độ góc : F= π4(mm

s )

+ Gia tốc tăng/giảm tốc : A=D=π( mm

s2 )+ Chu kỳ nội suy : T ns =5(ms)

Có bán kính:

R=√ ¿¿

- Vì đang sử dụng góc để tính toán chuyển đổi đơn vị của A,D,F như sau:

+ Góc quay : θ=180(ứng vớinửa đườngtròn)

 Số bước di chuyển cần thiết với tốc độ góc ω : k1= θ ω∗T

ns=4000+ Thời gian tăng/giảm tốc cần : t a =t d = ω A = ω D =0.25(s)

 Số bước cần thiết cho quá trình tăng/giảm tốc : m= t a

T ns= t d

T ns= 0.25 0.005=50

Áp dụng công thức (*) có:

Với trục X : ∆ X0(k)=∆ X0(k−1)+ 1

m (∆ X(k)−∆ X(k−m))Với trục Y : ∆ Y0(k)=∆ Y0(k−1)+ 1m(∆ Y(k)−∆ Y(k−m))

Với vận tốc góc: ω0(k)=ω0(k−1)+ 1m (ω(k)−ω(k−m))

Hình 4.6 Vận tốc quỹ đạo

Hình 4.7 Giá trị đặt theo các trục

Hình 4.8 Vị trí theo các trục

Hình 4.9 So sánh giá trị đặt và kết quả sau nội suy

Code Matlab

Nội suy đường tròn

%thông số

xA=200 ; yA=400 ; % Tọa độ điểm bắt đầu

xB=800 ; yB=400 ; % Tọa độ điểm kết thúc

xI=500 ; yI=400 ; % Tọa độ tâm

shortline=1;% 1 nếu đi theo cung ngắn, 0 nếu đi theo cung dài

clockwise=1;% 1 nếu đi cùng chiều kim đồng hồ, 0 nếu đi cùng chiều kim đồng hồ

A=50; % Gia tốc tang tốc

D=50; % Gia t?c giảm tốc

F=100; % tốc độ ăn dao

Tns=5e-3; % Chu kì nội suy

Tdk=1e-3; % Chu kì điều khiển

X(i+1)= X(i)+ dX_new(i);

Y(i+1)= Y(i)+ dY_new(i);

%% Thoi gian tang toc, giam toc

Tns=5e-3; % Chu ki noi suy

Tdk=1e-3; % Chu ki dieu khien

kT=5; % Ti so Tns/Tdk

F=50; % Toc do an dao

A=25; % Gia toc tang toc

D=25; % Gia toc giam toc

% Toa do diem A(start) va B(end)

Ta1=F/A; % Thoi gian tang toc

Td1=F/D; % Thoi gian giam toc

Tc1=(L1-F^2/(2*A)-F^2/(2*D))/F; % Thoi gian chuyen dong deu

else

F=sqrt(L1/(1/(2*A)+1/(2*D)));

Ta1=F/A; % Thoi gian tang toc

Td1=F/D; % Thoi gian giam toc

Tc1=0; % Thoi gian chuyen dong deu

end

Tsum1=Ta1+Tc1+Td1% Tong thoi gian chuyen dong

Na1=int32(Ta1/Tns); % So lan noi suy trong giai doan tang toc

Nd1=int32(Td1/Tns); % So lan noi suy trong giai doan chuyen dong deu

Nc1=int32(Tc1/Tns); % So lan noi suy trong giai doan giam toc

Nsum1=Na1+Nc1+Nd1; % Tong so lan noi suy

time_ns1=linspace(0,Tsum1,Nsum1);

time_dk1=linspace(0,Tsum1,kT*Nsum1);

%% Noi suy tho

Va1=linspace(0,F,Na1); % Toc do quang duong tang toc

Vc1=ones(1,Nc1)*F; % Toc do quang duong toc do khong doi

Vd1=linspace(F,0,Nd1); % Toc do quang duong giam toc

V1=[Va1 Vc1 Vd1]; % Toc do tren toan quy dao

VX1=V1*cosphi1; % Toc do ung voi truc X

VY1=V1*sinphi1; % Toc do ung voi truc Y

dL1=V1*Tns; % Quang duong di duoc sau moi Tns

dX1=dL1*cosphi1; % Quang duong di duoc theo truc X

dY1=dL1*sinphi1; % Quang duong di duoc theo truc Y

X_tho1 = xa1; Y_tho1 = ya1;

xA=200 ; yA=400 ; % Tọa độ điểm bắt đầu

xB=400 ; yB=400 ; % Tọa độ điểm kết thúc

xI=300 ; yI=400 ; % Tọa độ tâm

shortline2=1;% 1 nếu đi theo cung ngắn, 0 nếu đi theo cungdài

clockwise2=1;% 1 nếu đi cùng chiều kim đồng hồ, 0 nếu đi ngược chiều kim đồng hồ

% Kiểm tra dạng quỹ đạo là normal block hay short block

for i= 1:Nsum2

X2(i+1)=X2(i)+R*(cosd(alpha2(i+1))-cosd(alpha2(i))); Y2(i+1)=Y2(i)+R*(sind(alpha2(i+1))-sind(alpha2(i))); dX2(i)=X2(i+1)-X2(i);

X2(i+1)= X2(i)+ dX_new2(i);

Y2(i+1)= Y2(i)+ dY_new2(i);

Ta3=F/A; % Thoi gian tang toc

Td3=F/D; % Thoi gian giam toc

Tc3=(L3-F^2/(2*A)-F^2/(2*D))/F; % Thoi gian chuyen dong deu

else

F3=sqrt(L3/(1/(2*A)+1/(2*D)));

Ta3=F/A; % Thoi gian tang toc

Td3=F/D; % Thoi gian giam toc

Tc3=0; % Thoi gian chuyen dong deu

end

Tsum3=Ta3+Tc3+Td3% Tong thoi gian chuyen dong

Na3=int32(Ta3/Tns); % So lan noi suy trong giai doan tang toc

Nd3=int32(Td3/Tns); % So lan noi suy trong giai doan chuyen dong deu

Nc3=int32(Tc3/Tns); % So lan noi suy trong giai doan giam toc

Nsum3=Na3+Nc3+Nd3; % Tong so lan noi suy

time_ns=linspace(0,Tsum3,Nsum3);

time_dk=linspace(0,Tsum3,kT*Nsum3);

%% Noi suy tho

Va3=linspace(0,F,Na3); % Toc do quang duong tang toc

Vc3=ones(1,Nc3)*F; % Toc do quang duong toc do khong doi

Vd3=linspace(F,0,Nd3); % Toc do quang duong giam toc

V3=[Va3 Vc3 Vd3]; % Toc do tren toan quy dao

VX3=V3*cosphi3; % Toc do ung voi truc X

VY3=V3*sinphi3; % Toc do ung voi truc Y

dL3=V3*Tns; % Quang duong di duoc sau moi Tns

dX3=dL3*cosphi3; % Quang duong di duoc theo truc X

dY3=dL3*sinphi3; % Quang duong di duoc theo truc Y

X_tho3 = xa3; Y_tho3 = ya3;

xA4=400 ; yA4=200 ; % Tọa độ điểm bắt đầu

xB4=200 ; yB4=200 ; % Tọa độ điểm kết thúc

xI4=300 ; yI4=200 ; % Tọa độ tâm

shortline4=1;% 1 nếu đi theo cung ngắn, 0 nếu đi theo cungdài

clockwise4=1;% 1 nếu đi cùng chiều kim đồng hồ, 0 nếu đi ngược chiều kim đồng hồ

Tns=5e-3; % Chu kì nội suy

Tdk=1e-3; % Chu kì điều khiển

% Kiểm tra quỹ đạo là normal block hay short block

X4(i+1)=X4(i)+R*(cosd(alpha4(i+1))-cosd(alpha4(i))); Y4(i+1)=Y4(i)+R*(sind(alpha4(i+1))-sind(alpha4(i))); dX4(i)=X4(i+1)-X4(i);

X4(i+1)= X4(i)+ dX_new4(i);

Y4(i+1)= Y4(i)+ dY_new4(i);