Báo cáo bài tập lớn môn hệ thống Điều khiển máy cnc bài 1 thiết kế Điều khiển cho 2 Động cơ

Yêu cầu đề bài Một hệ thống CNC gồm 2 trục X và Y được điều khiển bởi 2 động cơ có bộ driver điều khiển tốc độ kèm theo.. Tự chọn thông số 02 hàm truyền trên và thiết kế bộ điều khiển vị

Thiết kế điều khiển cho 2 động cơ

Chọn hệ số khuếch đại 2 khâu quán tính bậc nhất là 𝑘 1 = 5𝑘 2 = 1; hằng số thời gian 𝑇 1 = 0.02; 𝑇 2 = 0.01

Do hàm truyền giữa tốc độ và điện áp là 1 khâu quán tính bậc nhất, hàm truyền giữa vị trí và điện áp sẽ là 1 khâu tích phân quán tính bậc nhất:

Ta sử dụng bộ điều khiển PI Cấu trúc điều khiển được trình bày ở hình 1.1, khối Saturation để giới hạn đầu ra bộ điều khiển ở 10V

Hình 1.1: Sơ đồ mô phỏng trên Simulink

Sử dụng phương pháp tối ưu đối xứng Xét đối tượng có hàm truyền:

Bộ điều khiển PI có hàm truyền:

Giả sử độ quá điều chỉnh mong muốn là ∆ℎ Xác định tham số 𝑎 theo phương trình:

Các tham số 𝑘 𝑝 , 𝑇 𝑖 được xác định như sau:

𝑘 𝑝 = 1 𝑘𝑇√𝑎 Áp dụng với đối tượng là 2 động cơ đã chọn, sử dụng Matlab để tính toán và thu được bộ thông số cho 2 bộ điều khiển:

Với thông số bộ điều khiển như trên, đáp ứng vị trí của 2 động cơ thu được như trên Hình 1.2 và 1.3

Hình 1.2 Đáp ứng vị trí của 2 động cơ

6 Điện áp của 2 động cơ như Hình 1.3

Hình 1.3 Đồ thị điện áp 2 động cơ Nhận xét:

 Giá trị vị trí 2 động cơ đã bám sát giá trị đặt, sai lệch tĩnh bằng 0

 Độ quá điều chỉnh bằng 3%, đúng với tính toán

 Điện áp 2 động cơ không vượt quá 10V

Nội suy ADCBI thô và tinh cho đường thẳng và đường tròn

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 với các thông số cần sử dụng trong quá trình nội suy:

 Tọa độ điểm bắt đầu: 𝐴(200; 200)

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

 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)

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

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 Short Block : 𝐹

2𝐷 − 𝐿 = −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

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

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

Hình 2.1.1 Sơ đồ nội suy đường thẳng 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à: 𝑉(𝑖 + 1) = 𝑉(𝑖) + 𝐴 𝑇 𝑛𝑠

 Quãng đường đi được sau mỗi chu kỳ nội suy là: ∆𝐿 𝑎 = 𝑉(𝑖) 𝑇 𝑛𝑠

 Độ dịch chuyển trên trục X: ∆𝑋(𝑖) = ∆𝐿 𝑎 𝐶𝑜𝑠(𝜑)

 Độ dịch chuyển trên trục Y: ∆𝑌(𝑖) = ∆𝐿 𝑎 𝑆𝑖𝑛(𝜑)

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

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 :

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 gia tốc D Tương tự, ta xác định được :

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à: 𝑁 𝑆 = 𝑁 𝐴 + 𝑁 𝐶 + 𝑁 𝐷

Do chu kì điều khiển 𝑇 đ𝑘 nhỏ hơn chu kì nội suy 𝑇 𝑛𝑠 5 lần nên sau khi nội suy tinh, kích thước của 2 ma trận mới 𝑑𝑋 𝑛𝑒𝑤 , 𝑑𝑌 𝑛𝑒𝑤 sẽ tăng gấp 5 lần so với 2 ma trận cũ dX, dY

Hình 2.1.2: Chu kì nội suy và chu kì điều khiển

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

Hình 2.1.3: Độ dịch chuyển trong mỗi chu kì nội suy

 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

Sử dụng Matlab để mô phỏng, ta thu được kết quả:

Hình 2.1.4: Quỹ đạo đường thẳng nội suy thô và tinh Đồ thị vị trí khi nội suy tinh:

Hình 2.1.5: Toạ độ trục X khi nội suy tinh

Hình 2.1.6: Toạ độ trục Y khi nội suy tinh Đồ thị vận tốc:

Hình 2.1.6: Vận tốc các trục khi nội suy tinh

13 Đưa quỹ đạo đã thiết kế vào hệ 2 động cơ ở Bài 1, ta thu được kết quả:

Hình 2.1.7: Đáp ứng vị trí của 2 động cơ với quỹ đạo nội suy tinh

Hình 2.8: Quỹ đạo nội suy và quỹ đạo thực

Nội suy đường tròn

Cần tìm toạ độ tâm và góc quay dựa trên điểm đầu, điểm cuối và chiều quay do người sử dụng nhập:

Xét cung tròn có điểm đầu là 𝐴(𝑥 𝐴 ; 𝑦 𝐴 ), điểm cuối là 𝐵(𝑥 𝐵 ; 𝑦 𝐵 ), bán kính 𝑅 Đường trung trực của AB có phương trình:

2 ) là trung điểm đoạn AB; (𝑥 𝐴𝐵 ; 𝑦 𝐴𝐵 ) = (𝑥 𝐵 − 𝑥 𝐴 ; 𝑦 𝐵 − 𝑦 𝐴 ) là toạ độ vector AB

Tâm O có toạ độ (𝑥 𝑂 ; 𝑦 𝑂 ) thuộc trung trực AB nên:

Do 𝑂𝐴 = 𝑅 nên ta có phương trình thứ 2:

Thay 𝑥 𝑂 theo 𝑦 𝑂 và giải phương trình bậc 2, ta được 2 nghiệm tương ứng với toạ độ 2 tâm Gọi 𝑂 1 (𝑥 𝑂1 ; 𝑦 𝑂1 ) là nghiệm “nằm dưới” (có tung độ nhỏ hơn) và

𝑂 2 (𝑥 𝑂2 ; 𝑦 𝑂2 ) là nghiệm còn lại Tuỳ vào chiều quay mà ta sẽ chọn tâm là 𝑂 1 hay 𝑂 2 sao cho góc quay luôn nhỏ hơn 180 độ

Hình 2.2.1: Vị trí 2 tâm thoả mãn đề bài

Gọi 𝑡ℎ𝑒𝑡𝑎𝑆 là góc của vector 𝑂 1 𝐴 so với trục hoành, 𝑡ℎ𝑒𝑡𝑎𝐸 là góc của vector

𝑂 1 𝐵 so với trục hoành Khi đó 𝑡ℎ𝑒𝑡𝑎1 = 𝑡ℎ𝑒𝑡𝑎𝐸 − 𝑡ℎ𝑒𝑡𝑎𝑆 là góc quay từ 𝑂 1 𝐴 đến

Hình 2.2.2: Cách tính góc 𝑡ℎ𝑒𝑡𝑎1 Xét 2 trường hợp của 𝑡ℎ𝑒𝑡𝑎1 như hình vẽ:

Hình 2.2.3: Hai trường hợp của 𝑡ℎ𝑒𝑡𝑎1 Nhận xét:

 Nếu 𝑡ℎ𝑒𝑡𝑎1 > 0; tâm sẽ là 𝑂 1 khi 𝐶𝑙𝑘 = 0 và là 𝑂 2 khi 𝐶𝑙𝑘 = 1

 Nếu 𝑡ℎ𝑒𝑡𝑎1 < 0; tâm sẽ là 𝑂 2 khi 𝐶𝑙𝑘 = 0 và là 𝑂 1 khi 𝐶𝑙𝑘 = 1

Với cách chọn tâm như trên, góc quay sẽ luôn nhỏ hơn 180 Khi đó góc quay có thể được tính:

2.2.2 Thực hiện nội suy đường tròn

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

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

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

 Tính toán thời gian tăng tốc (T A ), giảm tốc (T D ), vận tốc không đổi (T C ) :

Hình 2.2.4: Nội suy thô đường tròn

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à Tns:

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 𝑇 đ𝑘 nhỏ hơn chu kì nội suy 𝑇 𝑛𝑠 5 lần nên sau khi nội suy tinh, kích thước 𝑑𝑋 𝑛𝑒𝑤 , 𝑑𝑌 𝑛𝑒𝑤 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:

Hình 2.2.5: Nội suy tinh đường tròn Sau khi nội suy tinh xong, vị trí 2 trục X,Y sẽ được tính như sau:

Thực hiện mô phỏng trên Matlab, ta thu được kết quả nội suy

Hình 2.2.6: Quỹ đạo đường tròn khi nội suy thô và tinh

Vận tốc theo các trục:

Hình 2.2.7: Tốc độ các trục X,Y khi nội suy tinh đường tròn

Hình 2.2.8: Tốc độ góc khi nội suy tinh đường tròn Đưa quỹ đạo đã thiết kế vào hệ 2 động cơ ở Bài 1, ta thu được kết quả:

Hình 2.2.9: Đáp ứng vị trí 2 động cơ khi nội suy tinh đường tròn

Hình 2.2.10: Quỹ đạo nội suy và quỹ đạo thực

Thực hiện nội suy chữ O và P

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

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

Hình 3.1.1 Quỹ đạo chữ O theo tính toán lí thuyết

 Đường thứ nhất: Nội suy đường tròn từ điểm đầu D (161,668 ; 55,555) đến điểm cuối A (161,668 ; 344,445) với tâm I 1 (300 ; 200) theo cung tròn ngắn, chiều thuận kim đồng hồ

 Đường thứ hai: Nội suy đường tròn từ điểm đầu A (161,668 ; 344,445) đến điểm cuối B (338,332 ; 344,445), với tâm I 2 (250 ; 256), theo cung tròn ngắn, chiều thuận kim đồng hồ

 Đường thứ ba: Nội suy đường tròn từ điểm đầu B (338,332 ; 344,445) đến điểm cuối C(338,332 ; 55,555) với tâm I 3 (200 ; 200), theo cung tròn ngắn, chiều thuận kim đồng hồ

 Đường thứ tư: Nội suy đường tròn từ điểm đầu C (338,332 ; 55,555) đến điểm cuối D (161,668 ; 55,555), với tâm I 4 (250 ; 144), theo cung tròn ngắn, chiều thuận kim đồng hồ

Thực hiện nội suy các đường với phương pháp giống Bài 2, ta thu được kết quả:

Hình 3.1.2: Kết quả nội suy chữ O

23 Đưa các giá trị quỹ đạo theo trục X và Y vào mô hình động cơ ở Bài 1, ta thu được đáp ứng:

Hình 3.1.3: Đáp ứng vị trí của 2 động cơ khi vẽ chữ O

Hình 3.1.4: Quỹ đạo nội suy và quỹ đạo thực

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

Hình 3.2.1: Quỹ đạo chữ P theo tính toán lý thuyết

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

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

 Đường thứ hai: Nội suy đường thẳng từ điểm đầu B (0; 800) đến điểm cuối

 Đường thứ ba: Nội suy đường tròn từ điểm đầu C (200; 800) đến điểm cuối D(200 ; 400) với tâm I (200 ; 600), theo cung tròn ngắn, chiều thuận kim đồng hồ

 Đường thứ tư: Nội suy đường thẳng từ điểm đầu D(200 ; 400) đến điểm cuối

Thực hiện nội suy các đường với phương pháp giống Bài 2, ta thu được kết quả:

Hình 3.2.2: Kết quả nội suy chữ P Đưa các giá trị quỹ đạo theo trục X và Y vào mô hình động cơ ở Bài 1, ta thu được đáp ứng

Hình 3.2.3: Đáp ứng vị trí 2 động cơ khi vẽ chữ P

Hình 3.2.4: Quỹ đạo nội suy và quỹ đạo thực

Nội suy ADCAI thô cho đường thẳng và đường tròn

Nội suy theo quỹ đạo đường thẳng

Hình 4.1.1: Lưu đồ thuật toán nội suy thô đường thẳng Đầu tiên, thực hiện nội suy thô đường thẳng với tốc độ bằng hằng số Trong mỗi chu kì nội suy 𝑇 𝑖𝑝𝑜 , độ dịch chuyển 2 trục X,Y lần lượt là ∆𝑥 và ∆𝑦 được tính như sau:

Tổng thời gian nội suy:

𝑇 = 𝐿𝐹Với 𝐿 là tổng quãng đường di chuyển, 𝐹 là tốc độ ăn dao

Số chu kì nội suy:

Số xung mỗi trục trong 1 chu kì nội suy:

𝐵𝐿𝑈 𝑁 Độ dịch chuyển mỗi trục trong mỗi chu kì nội suy:

Tiếp theo, ta thực hiện tăng giảm tốc bằng cách sử dụng bộ lọc số Theo lý thuyết bộ lọc kỹ thuật số, nếu tín hiệu đầu vào x[n] được đưa vào bộ lọc với đáp ứng xung h[n], tín hiệu đầu ra y [n] được biểu thị bằng tích chập của h[n] và x[n]

Tích chập chung của f1[n] và f2[n] cho một hệ thống rời rạc:

Giả sử rằng x[n] là đầu ra của nội suy thô và h [n] là bộ lọc số Ta thu được kết quả như hình dưới

Hàm h[n] có n là số chu kì, thời gian tăng giảm tốc được tính theo công thức

𝜏 = n ∗ 𝑇 𝑖𝑝𝑜 , từ thời gian tăng giảm tốc sẽ tính được số chu kì của bộ lọc số (coi thời gian tang tốc bằng thời gian giảm tốc) Giá trị mỗi xung của bộ lọc số có giá trị bằng 1/𝑛

Số chu kì của bộ lọc số : n = 𝑇 𝑎𝑐𝑐 /𝑇 𝑖𝑝𝑜

Các giá trị sau khi đi qua bộ lọc số được tính theo công thức: Δ𝑋 (𝑖) = Δ𝑋 (𝑖−1) + 𝑋 (𝑖) − 𝑋 (𝑖−𝑛)

𝑛 Thực hiện mô phỏng trên Matlab với các thông số:

 Gia tốc tăng tốc cho phép: 𝐴 = 30 𝑚𝑚/𝑠 2

 Gia tốc giảm tốc cho phép: 𝐷 = 30 𝑚𝑚/𝑠 2

Ta thu được kết quả:

Hình 4.1.2: Quỹ đạo đường thẳng khi nội suy ADCAI

Hình 4.1.3: Độ dịch chuyển theo các trục X,Y trước và sau bộ lọc Đưa các giá trị quỹ đạo theo trục X và Y vào mô hình động cơ ở Bài 1, ta thu được đáp ứng:

Hình 4.1.4: Đáp ứng vị trí 2 động cơ khi nội suy ADCAI đường thẳng

Hình 4.1.5: Quỹ đạo nội suy và quỹ đạo thực

Nội suy theo quỹ đạo đường tròn

Đầu tiên, thực hiện nội suy thô đường tròn:

Hình 4.2.1: Lưu đồ thuật toán nội suy thô đường tròn

Từ toạ độ điểm đầu, điểm cuối, bán kính và chiều quay, ta tìm tâm và góc quay bằng phương pháp đã trình bày ở Bài 2

Tính góc nội suy 𝑎 ở chu kì nội suy thứ 𝑖

Theo thuật toán Euler: với góc nội suy bất kì 𝜃 và bước dịch chuyển cơ bản (BLU)

Sau đó cập nhật lại giá trị 𝛼 = 𝜃

Hình 4.2.2: Mô tả một chu kì nội suy

Tăng giá trị của 𝛼 dần và tính độ dịch chuyển trên mỗi trục X, Y ở lần nội suy thứ i theo công thức:

Nội suy ngược chiều kim đồng hồ:

Nội suy thuận chiều kim đồng hồ:

Trong đó: 𝑡ℎ𝑒𝑡𝑎0 : góc nội suy ban đầu

R: bán kính đường tròn nội suy Tiếp theo, thực hiện tăng giảm tốc sử dụng bộ lọc số với phương pháp tương tự đường thẳng đã trình bày ở trên

Thực hiện mô phỏng trên Matlab với các thông số

 Gia tốc tăng tốc cho phép: 𝐴 = 30 𝑚𝑚/𝑠 2

 Gia tốc giảm tốc cho phép: 𝐷 = 30 𝑚𝑚/𝑠 2

 Tốc độ ăn dao (tốc độ dài): 𝐹 = 60 𝑚𝑚/𝑠 2

 Nội suy thuận chiều kim đồng hồ

Ta thu được kết quả:

Hình 4.2.3: Quỹ đạo đường tròn khi nội suy ADCAI

Hình 4.2.4: Độ dịch chuyển các trục X,Y trước và sau nội suy Đưa các giá trị quỹ đạo theo trục X và Y vào mô hình động cơ ở Bài 1, ta thu được đáp ứng:

Hình 4.2.5: Đáp ứng vị trí của 2 động cơ khi nội suy ADCAI đường tròn

Hình 4.2.6: Quỹ đạo nội suy và quỹ đạo thực

%Thong so ham truyen 2 dong co k1=5;

%Tinh toan bdk theo pp toi uu doi xung

%BDK1 delta_h1=0.03; % do qua dieu chinh a1= (4/(1+(pi^2)/(log(delta_h1))^2))^2

%BDK2 delta_h2=0.03;% do qua dieu chinh a2= (4/(1+(pi^2)/(log(delta_h2))^2))^2

%% Thoi gian tang toc, giam toc

Tns^-3; % Chu ki noi suy

Tdk-3; % Chu ki dieu khien kT=5; % Ti so Tns/Tdk

AP; % Gia toc tang toc

DP; % Gia toc giam toc

% Toa do diem A(start) va B(end) xa0;ya0;

Xstart=xa; Ystart=ya; %dong nay de dua vao gia tri ban dau cho simulink xb00;ybP0;

L=sqrt(Lx^2+Ly^2); cosphi=(xb-xa)/L; sinphi=(yb-ya)/L;

% Kiem tra dang quy dao la Normal block hay Short block if F^2/(2*A)+F^2/(2*D) < L

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 dong deu else

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 tang toc

Nd=int32(Td/Tns); % So lan noi suy trong giai doan chuyen dong deu Nc=int32(Tc/Tns); % So lan noi suy trong giai doan giam toc

Nsum=Na+Nc+Nd; % Tong so lan noi suy time_ns=linspace(0,Tsum,Nsum); time_dk=linspace(0,Tsum,kT*Nsum);

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

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

Vd=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; for j=2:length(dL)

X_tho(end+1)=X_tho(end)+dX(j);

Y_tho(end+1) =Y_tho(end)+dY(j); end

X_tho_C = X_tho(Na+1:Na+Nc);

X_tho_D = X_tho(Na+Nc+1:end);

Y_tho_C = Y_tho(Na+1:Na+Nc);

Y_tho_D = Y_tho(Na+Nc+1:end);

%% Noi suy tinh dX_new=[]; dY_new=[];

% Cap nhat gia tri noi suy tinh dX_new, dY_new for i=1:length(dL) aX=ones(1,kT)*dX(i)/kT; aY=ones(1,kT)*dY(i)/kT; dX_new=[dX_new aX]; dY_new=[dY_new aY]; end

% Tinh ra gia tri noi suy tinh X, Y

X=xa;Y=ya; for i=2:length(dX_new)

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

% Cap nhat so buoc noi suy tinh

Xc=X(Na_new+1:Na_new+Nc_new);

Xd=X(Na_new+Nc_new+1:end);

Yc=Y(Na_new+1:Na_new+Nc_new);

Yd=Y(Na_new+Nc_new+1:end);

VX_C=VX(Na+1:Na+Nc);

VX_D=VX(Na+Nc+1:end);

VY_C=VY(Na+1:Na+Nc);

VY_D=VY(Na+Nc+1:end); time_ns_A=time_ns(1:Na); time_ns_C=time_ns(Na+1:Na+Nc); time_ns_D=time_ns(Na+Nc+1:end); time_dk_A=time_dk(1:Na_new); time_dk_C=time_dk(Na_new+1:Na_new+Nc_new); time_dk_D=time_dk(Na_new+Nc_new+1:end);

T_simu=7 % thoi gian mo phong simulink sampleTime = 0.001; numSteps = 6470; time1 = sampleTime*(0:numSteps-1); time1 = time1'; data1 = X; simin1 = timeseries(data1,time1); time2 = sampleTime*(0:numSteps-1); time2 = time2'; data2 = Y; simin2 = timeseries(data2,time2); xmin=0; xmax50; %gioi han khoi xy graph ymin=0; ymax`0;

%% figure1 figure subplot(2,1,1); plot(time_dk_A,Xa,'r',time_dk_C,Xc,'k',time_dk_D,Xd,'b','LineWidth',1) ylabel('X (mm)') xlabel('Time (s)') title('Vi tri truc X') grid on subplot(2,1,2); plot(time_dk_A,Ya,'r',time_dk_C,Yc,'k',time_dk_D,Yd,'b','LineWidth',1) ylabel('Y (mm)') xlabel('Time (s)') title('Vi tri truc Y') grid on figure subplot(2,1,1); plot(time_ns_A,VX_A,'r',time_ns_C,VX_C,'k',time_ns_D,VX_D,'b','LineWidth',1) ylabel('vX (mm/s)') xlabel('Time (s)') title('Van toc truc X')

41 grid on subplot(2,1,2); plot(time_ns_A,VY_A,'r',time_ns_C,VY_C,'k',time_ns_D,VY_D,'b','LineWidth',1) ylabel('vY (mm/s)') xlabel('Time (s)') title('Van toc truc Y') grid on

%% figure2 figure subplot(2,1,1); plot(X_tho_A,Y_tho_A,'r',X_tho_C,Y_tho_C,'k',X_tho_D,Y_tho_D,'b','LineWidth',

1) xlabel('Truc X(mm)') ylabel('Truc Y(mm)') title('Quy dao sau khi noi suy tho') grid on subplot(2,1,2); plot(Xa,Ya,'r',Xc,Yc,'k',Xd,Yd,'b','LineWidth',1) xlabel('Truc X (mm)') ylabel('Truc Y (mm)') legend({'Acc','Cons','Dec'}); title('Quy dao Noi suy tinh') grid on

%ns tinh va so sanh figure(4) subplot(1,1,1); plot(X,Y,'b','LineWidth',3); hold on plot(out.X,out.Y,'r ','LineWidth',3); legend('Qu? ??o n?i suy','Qu? ??o th?c') axis([xmin xmax ymin ymax]); axis on xlabel('X (mm)') ylabel('Y (mm)') title('Qu? ??o n?i suy ???ng th?ng') grid on

Clk=1; % 1 la quay theo chieu kim dong ho; 0 là nguoc

% tim tam va goc quay cua cung tron di tu A den B, ban kinh R

%toa do 2 diem va ban kinh xA=Xstart; yA=Ystart; xB=Xend; yB=Yend;

%tim trung diem M va vector AB xM=(xA+xB)/2; yM=(yA+yB)/2; xAB=xB-xA; yAB=yB-yA;

%tim phuong trinh duong trung truc AB, co duoc xO theo yO

%thay vao phuong trinh OB=R duoc phuong trinh bac 2 an x co he so a,b,c if(xA==xB) xO1=xM; xO2=xM; yO1=-sqrt(R^2-(xO1-xA)^2)+yA; yO2=sqrt(R^2-(xO1-xA)^2)+yA; else

%he so cua phuong trinh bac 2 an y a=1+(yAB/xAB)^2; b=-2*(yAB/xAB)*(yM*yAB/xAB+xM-xA)-2*yA; c=(yM*yAB/xAB+xM-xA)^2+yA^2-R^2;

%Giai pt bac 2, tim ra 2 tam d=b^2-4*a*c; yO1=(-b-sqrt(d))/(2*a) %tam nam duoi xO1=-(yAB/xAB)*(yO1-yM)+ xM yO2=(-b+sqrt(d))/(2*a) %tam nam tren xO2=-(yAB/xAB)*(yO2-yM)+ xM end

%tinh goc AOB thetaSos((xA-xO1)/R); % goc cua O1A so voi phuong ngang thetaEos((xB-xO1)/R); %goc cua O1B so voi phuong ngang thetaAB=thetaE-thetaS; % goc quay tu A den B voi tam O1

%chon tam sao so goc quay luon nho hon 180 do if(thetaAB