THÔNG TIN TÀI LIỆU
TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI TIỂU LUẬN CUỐI KỲ Hệ thống điều khiển máy CNC Nhóm sinh viên thực (Nhóm 5): Trần Anh Dũng - 20173773 Lý Ngọc Thuận - 20174249 Hoàng Anh Tú - 20174316 Giảng viên hướng dẫn: TS Dương Minh Đức Bộ môn: Tự động hóa cơng nghiệp Viện: Viện Điện HÀ NỘI, 5/2021 MỤC LỤC YÊU CẦU BÀI TOÁN VÀ LỰA CHỌN THAM SỐ THIẾT KẾ BỘ ĐIỀU KHIỂN VỊ TRÍ PID CHO TRỤC ĐỘNG CƠ 2.1 Các bước thiết kế điều khiển 2.2 Thiết kế điều khiển 2.2.1 Trục x 2.2.2 Trục y NỘI SUY ĐƯỜNG THẲNG 3.1 Cấu trúc điều khiển trung tâm 3.2 Look Ahead 3.3 Tính tốn thời gian tăng tốc, giảm tốc, vận tốc không đổi 3.4 Nội suy vận tốc 10 3.5 Nội suy lệnh đặt cho đường thẳng (trích mẫu liệu) 11 3.6 Nội suy tinh 13 3.6.1 Nội suy tuyến tính 14 3.6.2 Nội suy trung bình 15 3.6.3 Quỹ đạo sau nội suy tinh 17 3.7 Thực điều khiển vị trí 18 NỘI SUY ĐƯỜNG TRÒN 23 4.1 Tính tốn thời gian tăng tốc, giảm tốc, vận tốc không đổi 23 4.2 Nội suy vận tốc 23 4.3 Nội suy sai lệch đặt cho đường tròn 24 4.4 Nội suy tinh 26 THỰC HIỆN VẼ CHỮ “C” 33 NỘI SUY THÔ QUỸ ĐẠO VỚI TĂNG/GIẢM TỐC SAU NỘI SUY 39 6.1 Nội suy tuyến tính 39 6.2 Nội suy đường tròn 42 PHỤ LỤC 47 7.1 Chương trình nội suy đường thẳng (m-file) 47 7.2 Chương trình nội suy đường trịn (m-file) 49 7.3 Chương trình nội suy chữ C (m-file) 53 7.4 Nội suy thô đường thẳng với kiểu tăng/giảm tốc sau nội suy dạng hình thang 60 7.5 Nội suy thơ đường trịn với kiểu tăng/giảm tốc sau nội suy dạng hình thang 61 7.6 Chương trình điều khiển simulink 62 YÊU CẦU BÀI TOÁN VÀ LỰA CHỌN THAM SỐ Một hệ thống CNC gồm trục X Y điều khiển động có driver điều khiển tốc độ kèm theo Giả sử hàm truyền với đầu vào điện áp điều khiển, đầu tốc độ hệ truyền động trục X Y khâu quán tính bậc Tự chọn thông số 02 hàm truyền thiết kế điều khiển vị trí cho trục thỏa mãn yêu cầu sau: - Độ điều chỉnh ≤ 5% - Sai lệch tĩnh ≤ 0,1% - Thời gian xác lập nhỏ - | Uđk | ≤ 10V Nhóm em lựa chọn thông số hàm truyền sau: • Trục x: • Trục y: 𝑉𝑥 𝑈𝑥 𝑉𝑦 𝑈𝑦 = = 0.03𝑠+1 0.05𝑠+1 Thực nội suy đường thẳng đường tròn 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) Điểm đầu, điểm cuối, bán kính tùy chọn - Thực nội suy thô nội suy tinh - Tự chọn vận tốc tối đa, gia tốc tối đa - Chu kỳ nội suy thô 5ms, nội suy tinh 1ms Thực mô hệ thống di chuyển theo quỹ đạo hình chữ O C Trình bày nêu ví dụ minh họa (thơng số tùy chọn) nội suy thô đường thẳng đường trịn 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 sau nội suy) Các thông số cụ thể trình bày cụ thể phần BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co THIẾT KẾ BỘ ĐIỀU KHIỂN VỊ TRÍ PID CHO TRỤC ĐỘNG CƠ Với thông số hàm truyền động cơ: 𝑉𝑥 • Trục x: 𝑈𝑥 𝑉𝑦 • Trục y: 𝑈𝑦 = = 0.03𝑠+1 0.05𝑠+1 Chọn phương pháp thiết kế điều khiển vị trí: Vì hàm truyền có khâu tích phân hàm truyền loại bỏ sai lệch tĩnh Bởi vậy, điều khiển vị trí chọn PD với hàm truyền: 𝐾𝑐 (𝑠) = 𝐾𝑝 (1 + 𝑇𝑑 𝑠) 2.1 Các bước thiết kế điều khiển - B1: Đặt giá trị Setpoint tín hiệu Step, biên độ 𝑈𝑑𝑘𝑚𝑎𝑥 - B2: Đặt giá trị 𝐾𝑝 = - B3: Quan sát đáp ứng đầu trục quan sát 𝑈𝑑𝑘 , sau 𝑈𝑑𝑘 thỏa mãn giá 𝑆𝑒𝑡𝑝𝑜𝑖𝑛𝑡 , 𝐾𝑑 =0 trị cho phép tinh chỉnh hệ số 𝐾𝑑 điều khiển đạt thỏa mãn yêu cầu thời gian xác lập độ q điều chỉnh Hình Mơ hình mơ Matlab Simulink 2.2 Thiết kế điều khiển 2.2.1 Trục x Sau thực bước trên, ta thu điều khiển vị trí động theo trục x với thông số: 𝐾𝑝 = 10, 𝑇𝑑 = 0.005 BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co Ta thu kết mơ phỏng: Hình 2 Đáp ứng vị trí điều khiển theo trục x Nhận xét: ▪ Đáp ứng đầu động theo trục x bám sát giá trị đặt ▪ Có sai lệch tĩnh xấp xỉ từ thời điểm t = 0.45s ▪ Thời gian xác lập: 0,23s ▪ Độ điều chỉnh: 4.1% < 5% BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co Hình Điện áp điều khiển trục x Nhận xét: Điện áp điều khiển < 10V → Từ nhận xét ta kết luận: Bộ điều khiển vị trí theo trục x thỏa mãn yêu cầu đề 2.2.2 Trục y Sau thực bước trên, ta thu điều khiển vị trí động theo trục x với thông số: 𝐾𝑝 = 10, 𝑇𝑑 = 0.025 Ta thu kết mô phỏng: BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co Hình Đáp ứng vị trí điều khiển theo trục y Nhận xét: ▪ Đáp ứng đầu động theo trục y bám sát giá trị đặt ▪ Có sai lệch tĩnh xấp xỉ từ thời điểm t = 0.45s ▪ Thời gian xác lập: 0,24s ▪ Độ điều chỉnh: 4% < 5% Hình Điện áp điều khiển theo trục y Nhận xét: Điện áp điều khiển < 10V → Từ nhận xét ta kết luận: Bộ điều khiển vị trí theo trục x thỏa mãn yêu cầu đề BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co NỘI SUY ĐƯỜNG THẲNG Trong phần nội suy đường thẳng, nhóm em xin chọn nội suy từ điểm A(10,10) đến điểm B(50,40) 3.1 Cấu trúc điều khiển trung tâm Sử dụng cấu trúc ADCBI: thực tăng tốc/giảm tốc trước nội suy 3.2 Look Ahead • Quỹ đạo chuyển động phân chia thành khối • Tính tốn tốc độ điểm bắt đầu điểm kết thúc khối dựa vào khối thực sau Xác định dạng profile vận tốc dạng normal block (dạng hình thang) gồm trình: tăng tốc, vận tốc khơng đổi, giảm tốc 3.3 Tính tốn thời gian tăng tốc, giảm tốc, vận tốc không đổi Thơng số profile vận tốc: • Chiều dài: L= 50 (mm), từ A(10,10) đến B(50,40) • Gia tốc tăng tốc cho phép : A=2 (mm/s2) BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co • Gia tốc giảm tốc cho phép: D=2 (mm/s2) • Chu kỳ nội suy: =5 (ms) • Tốc độ ăn dao: F=5 (mm/s) Với profile vận tốc dạng normal block ta có cơng thức tính sau: Tăng tốc Giảm tốc Vận tốc khơng đổi 𝐹 𝐹 Tính tốn thời gian 𝐹2 𝐹2 𝐿− − 𝑇𝐴 = = 2.5(𝑠) 𝑇𝐷 = = 2.5(𝑠) 2𝐴 2𝐷 tăng tốc 𝑇𝐴 , giảm 𝐴 𝐷 𝑇𝐶 = 𝐹 tốc 𝑇𝐷 , vận tốc = 7.5(s) không đổi 𝑇𝐶 𝑉𝐴,𝑖+1 = 𝑉𝐴,𝑖 + 𝜏𝐴 Nội suy 𝑉𝐷,𝑖+1 = 𝑉𝐷,𝑖 − 𝜏𝐷 3.4 Nội suy vận tốc Sau q trình tính tốn nội suy ta thu đồ thị vận tốc sau: Giảm tốc Tăng tốc 10 BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co 𝑉𝐶 = 𝐹 BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co syms k X3=zeros(1,Nd); Y3=zeros(1,Nd); for k=2:Nd+1 X3(1,1)=X2(1,Nc+1); X3(1,k)=X3(1,k-1)+deltaX3(1,k); Y3(1,1)=Y2(1,Nc+1); Y3(1,k)=Y3(1,k-1)+deltaY3(1,k); end %tinh toan khoang tang toc ns tinh syms i j%ns tuyen tinh aX1=zeros(1,Na*Nt+1);aX1(1,Na*Nt+1)=deltaX1(1,Na+1)/Nt; aY1=zeros(1,Na*Nt+1);aY1(1,Na*Nt+1)=deltaY1(1,Na+1)/Nt; for i=1:Na for j=(Nt*i-Nt+1):(Nt*i) aX1(1,j)=deltaX1(1,i)/Nt; aY1(1,j)=deltaY1(1,i)/Nt; end end syms k %ns trung binh bX1=zeros(1,Na*Nt+1);bY1=zeros(1,Na*Nt+1); b1X1=zeros(1,Na*Nt+1);b1Y1=zeros(1,Na*Nt+1); b2X1=zeros(1,Na*Nt+1);b2Y1=zeros(1,Na*Nt+1); for k=2:Na*Nt-3 bX1(1,k)=(aX1(1,k+3)+aX1(1,k+2)+aX1(1,k+1)+aX1(1,k)+aX1(1,k-1))/Nt; b1X1(1,k)=(aX1(1,k+4)+aX1(1,k+3)+aX1(1,k+2)+aX1(1,k+1)+aX1(1,k))/Nt; b2X1(1,k)=(bX1(1,k)+b1X1(1,k))/2; bY1(1,k)=(aY1(1,k+3)+aY1(1,k+2)+aY1(1,k+1)+aY1(1,k)+aY1(1,k-1))/Nt; b1Y1(1,k)=(aY1(1,k+4)+aY1(1,k+3)+aY1(1,k+2)+aY1(1,k+1)+aY1(1,k))/Nt; b2Y1(1,k)=(bY1(1,k)+b1Y1(1,k))/2; end for k=Na*Nt-2:Na*Nt+1 bX1(1,k)=((k-Na*Nt+3)*aX1(1,Na*Nt+1)+(Nt-k+Na*Nt-3)*aX1(1,k))/Nt; b1X1(1,k)=((k-Na*Nt+4)*aX1(1,Na*Nt+1)+(Nt-k+Na*Nt-4)*aX1(1,k))/Nt; b2X1(1,k)=(bX1(1,k)+b1X1(1,k))/2; bY1(1,k)=((k-Na*Nt+3)*aY1(1,Na*Nt+1)+(Nt-k+Na*Nt-3)*aY1(1,k))/Nt; b1Y1(1,k)=((k-Na*Nt+4)*aY1(1,Na*Nt+1)+(Nt-k+Na*Nt-4)*aY1(1,k))/Nt; b2Y1(1,k)=(bY1(1,k)+b1Y1(1,k))/2; end syms l X11=zeros(1,Na*Nt+1);X12=zeros(1,Na*Nt+1); Y11=zeros(1,Na*Nt+1);Y12=zeros(1,Na*Nt+1); for l=2:Na*Nt+1 X11(1,1)=10;X12(1,1)=10; X11(1,l)=X11(1,l-1)+aX1(1,l);X12(1,l)=X12(1,l-1)+b2X1(1,l); Y11(1,1)=10;Y12(1,1)=10; Y11(1,l)=Y11(1,l-1)+aY1(1,l);Y12(1,l)=Y12(1,l-1)+b2Y1(1,l); end %tinh toan khoang cons ns tinh syms i j k aX2=zeros(1,Nc*Nt+1);aX2(1,Nc*Nt+1)=deltaX2(1,Nc+1)/Nt; aY2=zeros(1,Nc*Nt+1);aY2(1,Nc*Nt+1)=deltaY2(1,Nc+1)/Nt; X21=zeros(1,Nc*Nt+1);Y21=zeros(1,Nc*Nt+1); for i=1:Nc for j=(Nt*i-Nt+1):(Nt*i) aX2(1,j)=deltaX2(1,i)/Nt; aY2(1,j)=deltaY2(1,i)/Nt; end end for k=2:Nc*Nt+1 X21(1,1)=X11(1,Na*Nt+1); X21(1,k)=X21(1,k-1)+aX2(1,k); Y21(1,1)=Y11(1,Na*Nt+1); Y21(1,k)=Y21(1,k-1)+aY2(1,k); end %tinh toan khoang giam toc ns tinh 48 BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co syms i j%ns tuyen tinh aX3=zeros(1,Na*Nt+1); aX3(1,1)=deltaX3(1,1)/Nt; aY3=zeros(1,Na*Nt+1); aY3(1,1)=deltaY3(1,1)/Nt; for i=2:Nd for j=(Nt*(i-1)-Nt+2):(Nt*(i-1)+1) aX3(1,j)=deltaX3(1,i)/Nt; aY3(1,j)=deltaY3(1,i)/Nt; end end syms k %ns trung binh bX3=zeros(1,Nd*Nt+1);bY3=zeros(1,Nd*Nt+1); b1X3=zeros(1,Nd*Nt+1);b1Y3=zeros(1,Nd*Nt+1); b2X3=zeros(1,Nd*Nt+1);b2Y3=zeros(1,Nd*Nt+1); for k=1:4 bX3(1,k)=((5-k)*aX3(1,1)+(k-1)*aX3(1,k))/Nt; b1X3(1,k)=((6-k)*aX3(1,1)+(k)*aX3(1,k))/Nt; b2X3(1,k)=(bX3(1,k)+b1X3(1,k))/2; bY3(1,k)=((5-k)*aY3(1,1)+(k-1)*aY3(1,k))/Nt; b1Y3(1,k)=((6-k)*aY3(1,1)+(k)*aY3(1,k))/Nt; b2Y3(1,k)=(bY3(1,k)+b1Y3(1,k))/2; end for k=5:Nd*Nt bX3(1,k)=(aX3(1,k)+aX3(1,k-1)+aX3(1,k-2)+aX3(1,k-3)+aX3(1,k-4))/Nt; b1X3(1,k)=(aX3(1,k+1)+aX3(1,k)+aX3(1,k-1)+aX3(1,k-2)+aX3(1,k-3))/Nt; b2X3(1,k)=(bX3(1,k)+b1X3(1,k))/2; bY3(1,k)=(aY3(1,k)+aY3(1,k-1)+aY3(1,k-2)+aY3(1,k-3)+aY3(1,k-4))/Nt; b1Y3(1,k)=(aY3(1,k+1)+aY3(1,k)+aY3(1,k-1)+aY3(1,k-2)+aY3(1,k-3))/Nt; b2Y3(1,k)=(bY3(1,k)+b1Y3(1,k))/2; end syms l X31=zeros(1,Nd*Nt+1);X32=zeros(1,Nd*Nt+1); Y31=zeros(1,Nd*Nt+1);Y32=zeros(1,Nd*Nt+1); for l=2:Nd*Nt+1 X31(1,1)=X21(1,Nc*Nt+1);X32(1,1)=X21(1,Nc*Nt+1); X31(1,l)=X31(1,l-1)+aX3(1,l);X32(1,l)=X32(1,l-1)+b2X3(1,l); Y31(1,1)=Y21(1,Nc*Nt+1);Y32(1,1)=Y21(1,Nc*Nt+1); Y31(1,l)=Y31(1,l-1)+aY3(1,l);Y32(1,l)=Y32(1,l-1)+b2Y3(1,l); end %% X41=zeros(1,13500-12503); Y41=zeros(1,13500-12503); for t=1:(13500-12503) X41(1,t)=X31(1,Nd*Nt+1); Y41(1,t)=Y31(1,Nd*Nt+1); end %% X11s=X11(:, 1:2500);X21s=X21(:, 1:7500);X31s=X31(:, 1:2500); Y11s=Y11(:, 1:2500);Y21s=Y21(:, 1:7500);Y31s=Y31(:, 1:2500); wave.time = [];%iput simulink sum=[X11s,X21s,X31s,X41;Y11s,Y21s,Y31s,Y41]'; wave.signals.values = sum; wave.signals.dimensions =2; xsml=out.x_sml; ysml=out.y_sml; if(wave.time>12503) wave.signals.value=X32(2501,1); end 7.2 Chương trình nội suy đường tròn (m-file) %% clear sum F=5; %toc an dao 50mm/s^2 A=2; %gia toc tang toc cho phep D=2; %gia toc giam toc cho phep L=50; %chieu dai T=5e-3; %chu ky noi suy Tdk=1e-3; 49 BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co Nt=T/Tdk; I=(F*F)/(2*A)+(F*F)/(2*D)-L; %%tinh toan thoi gian tang toc giam toc Ta=F/A;%time acc Td=F/D;%time dec Sa=(F*F)/(2*A); Sd=(F*F)/(2*D); Tc=(L-Sa-Sd)/F;%time cons %%Noi suy Va0=0; Vd0=F; Na=Ta/T; Nd=Td/T; Nc=Tc/T; Va=Va0:T*A:F;%Vi+1=Vi+A*T Vd=Vd0:-T*D:0;%Vi+1=Vi-A*T Vc=ones(1,Nc+1)*F; ta=0:T:Ta; td=Tc+Ta:T:(Tc+Ta+Td); tc=Ta:T:(Ta+Tc); tas=0:Tdk:Ta; tds=Tc+Ta:Tdk:(Tc+Ta+Td); %noi suy duong tron X_tam=100/10; Y_tam=(310/6); R=1250/30; Wa=Va/R;%w=v/r toc tang toc alpha=Wa*T;%noi suy goc alpha tang toc tong=sum(alpha);%tong quang duong tang toc Wd=Vd/R;%toc giam toc alpha2=Wd*T;%noi suy goc alpha giam toc tong2=sum(alpha2);%tong quang duong giam toc Wc=F/R;%toc cons tong1=acos(7/25)-tong-tong2;%quang duong cons Tc_dtr=tong1/Wc;%Thoi gian cons Nc_dtr=round(Tc_dtr/T);% so lan noi suy cons Vc_dtr=ones(1,Nc_dtr+1)*F;%toc cons tc_dtr=Ta:T:(Ta+Tc_dtr);% ko de lam gi :) tcs=Ta:Tdk:(Ta+Tc_dtr); Wc_dtr=Vc_dtr/R;%w=v/r alpha1=Wc_dtr*T;% noi suy goc alpha cons td_dtr=Tc_dtr+Ta:T:(Tc_dtr+Ta+Td); %% %noi suy tinh acc syms i j%ns tuyen tinh a_alpha=zeros(1,Na*Nt+1);a_alpha(1,Na*Nt+1)=alpha(1,Na+1)/Nt; for i=1:Na for j=(Nt*i-Nt+1):(Nt*i) a_alpha(1,j)=alpha(1,i)/Nt; end end syms k %ns trung binh b_alpha=zeros(1,Na*Nt+1); b1_alpha=zeros(1,Na*Nt+1); b2_alpha=zeros(1,Na*Nt+1); for k=2:Na*Nt-3 b_alpha(1,k)=(a_alpha(1,k+3)+a_alpha(1,k+2)+a_alpha(1,k+1)+a_alpha(1,k)+a_alpha(1,k1))/Nt; b1_alpha(1,k)=(a_alpha(1,k+4)+a_alpha(1,k+3)+a_alpha(1,k+2)+a_alpha(1,k+1)+a_alpha(1,k ))/Nt; b2_alpha(1,k)=(b_alpha(1,k)+b1_alpha(1,k))/2; 50 BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co end for k=Na*Nt-2:Na*Nt+1 b_alpha(1,k)=((k-Na*Nt+3)*a_alpha(1,Na*Nt+1)+(Nt-k+Na*Nt-3)*a_alpha(1,k))/Nt; b1_alpha(1,k)=((k-Na*Nt+4)*a_alpha(1,Na*Nt+1)+(Nt-k+Na*Nt-4)*a_alpha(1,k))/Nt; b2_alpha(1,k)=(b_alpha(1,k)+b1_alpha(1,k))/2; end %noi suy tinh cons syms i j k a_alpha1=zeros(1,Nc_dtr*Nt+1);a_alpha1(1,Nc_dtr*Nt+1)=alpha1(1,Nc_dtr+1)/Nt; for i=1:Nc_dtr for j=(Nt*i-Nt+1):(Nt*i) a_alpha1(1,j)=alpha1(1,i)/Nt; end end %noi suy tinh dec syms i j%ns tuyen tinh a_alpha2=zeros(1,Nd*Nt+1);a_alpha2(1,1)=alpha2(1,1)/Nt; for i=2:Nd for j=(Nt*(i-1)-Nt+2):(Nt*(i-1)+1) a_alpha2(1,j)=alpha2(1,i)/Nt; end end syms k %ns trung binh b_alpha2=zeros(1,Nd*Nt+1); b1_alpha2=zeros(1,Nd*Nt+1); b2_alpha2=zeros(1,Nd*Nt+1); for k=1:4 b_alpha2(1,k)=((5-k)*a_alpha2(1,1)+(k-1)*a_alpha2(1,k))/Nt; b1_alpha2(1,k)=((6-k)*a_alpha2(1,1)+(k)*a_alpha2(1,k))/Nt; b2_alpha2(1,k)=(b_alpha2(1,k)+b1_alpha2(1,k))/2; end for k=5:Nd*Nt b_alpha2(1,k)=(a_alpha2(1,k)+a_alpha2(1,k-1)+a_alpha2(1,k-2)+a_alpha2(1,k3)+a_alpha2(1,k-4))/Nt; b1_alpha2(1,k)=(a_alpha2(1,k+1)+a_alpha2(1,k)+a_alpha2(1,k-1)+a_alpha2(1,k2)+a_alpha2(1,k-3))/Nt; b2_alpha2(1,k)=(b_alpha2(1,k)+b1_alpha2(1,k))/2; end %% %tinh toan dich chuyen ns tho %acc A_dtr=cos(alpha);%A=cos B_dtr=sin(alpha);%B=sin X4=zeros(1,Na+1); Y4=zeros(1,Na+1); syms z for z=2:Na+1 X4(1,1)=10;%Hoanh diem A Y4(1,1)=10;%Tung diem A X4(1,z)=A_dtr(1,z-1)*((X4(1,z-1))-X4(1,1))+B_dtr(1,z-1)*(Y_tam-Y4(1,z-1))+X4(1,1); Y4(1,z)=Y_tam-A_dtr(1,z-1)*(Y_tam-Y4(1,z-1))+B_dtr(1,z-1)*(X4(1,z-1)-X4(1,1)); end %cons A_dtr1=cos(alpha1); B_dtr1=sin(alpha1); X5=zeros(1,Nc_dtr+1); Y5=zeros(1,Nc_dtr+1); syms v for v=2:Nc_dtr+1 X5(1,1)=X4(1,Na+1); Y5(1,1)=Y4(1,Na+1); X5(1,v)=A_dtr1(1,v-1)*(X5(1,v-1)-X4(1,1))+B_dtr1(1,v-1)*(Y_tam-Y5(1,v-1))+X4(1,1); Y5(1,v)=Y_tam-A_dtr1(1,v-1)*(Y_tam-Y5(1,v-1))+B_dtr1(1,v-1)*(X5(1,v-1)-X4(1,1)); end %dec A_dtr2=cos(alpha2); 51 BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co B_dtr2=sin(alpha2); X6=zeros(1,Nd+1); Y6=zeros(1,Nd+1); X6_dtr=zeros(1,Nd+1); Y6_dtr=zeros(1,Nd+1); syms m for m=2:Nd+1 X6(1,1)=X5(1,Nc_dtr+1); Y6(1,1)=Y5(1,Nc_dtr+1); X6(1,m)=A_dtr2(1,m-1)*(X6(1,m-1)-X4(1,1))+B_dtr2(1,m-1)*(Y_tam-Y6(1,m-1))+X4(1,1); Y6(1,m)=Y_tam-A_dtr2(1,m-1)*(Y_tam-Y6(1,m-1))+B_dtr2(1,m-1)*(X6(1,m-1)-X4(1,1)); end %% %tinh toan dich chuyen ns tinh tuyen tinh %acc A1_dtr=cos(a_alpha);%A=cos B1_dtr=sin(a_alpha);%B=sin X41=zeros(1,Na*Nt+1); Y41=zeros(1,Na*Nt+1); syms z for z=2:Na*Nt+1 X41(1,1)=10;%Hoanh diem A Y41(1,1)=10;%Tung diem A X41(1,z)=A1_dtr(1,z-1)*((X41(1,z-1))-X41(1,1))+B1_dtr(1,z-1)*(Y_tam-Y41(1,z1))+X41(1,1); Y41(1,z)=Y_tam-A1_dtr(1,z-1)*(Y_tam-Y41(1,z-1))+B1_dtr(1,z-1)*(X41(1,z-1)X41(1,1)); end %cons A1_dtr1=cos(a_alpha1); B1_dtr1=sin(a_alpha1); X51=zeros(1,Nc_dtr*Nt+1); Y51=zeros(1,Nc_dtr*Nt+1); syms v for v=2:Nc_dtr*Nt+1 X51(1,1)=X41(1,Na*Nt+1); Y51(1,1)=Y41(1,Na*Nt+1); X51(1,v)=A1_dtr1(1,v-1)*(X51(1,v-1)-X41(1,1))+B1_dtr1(1,v-1)*(Y_tam-Y51(1,v1))+X41(1,1); Y51(1,v)=Y_tam-A1_dtr1(1,v-1)*(Y_tam-Y51(1,v-1))+B1_dtr1(1,v-1)*(X51(1,v-1)X41(1,1)); end %dec A1_dtr2=cos(a_alpha2); B1_dtr2=sin(a_alpha2); X61=zeros(1,Nd*Nt+1); Y61=zeros(1,Nd*Nt+1); syms m for m=2:Nd*Nt+1 X61(1,1)=X51(1,Nc_dtr*Nt+1); Y61(1,1)=Y51(1,Nc_dtr*Nt+1); X61(1,m)=A1_dtr2(1,m-1)*(X61(1,m-1)-X41(1,1))+B1_dtr2(1,m-1)*(Y_tam-Y61(1,m1))+X41(1,1); Y61(1,m)=Y_tam-A1_dtr2(1,m-1)*(Y_tam-Y61(1,m-1))+B1_dtr2(1,m-1)*(X61(1,m-1)X41(1,1)); end %% %tinh toan dich chuyen ns tinh trung binh %acc A2_dtr=cos(b2_alpha);%A=cos B2_dtr=sin(b2_alpha);%B=sin X42=zeros(1,Na*Nt+1); Y42=zeros(1,Na*Nt+1); syms z for z=2:Na*Nt+1 X42(1,1)=10;%Hoanh diem A 52 BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co Y42(1,1)=10;%Tung diem A X42(1,z)=A2_dtr(1,z-1)*((X42(1,z-1))-X42(1,1))+B2_dtr(1,z-1)*(Y_tam-Y42(1,z1))+X42(1,1); Y42(1,z)=Y_tam-A2_dtr(1,z-1)*(Y_tam-Y42(1,z-1))+B2_dtr(1,z-1)*(X42(1,z-1)X42(1,1)); end %cons A2_dtr1=cos(a_alpha1); B2_dtr1=sin(a_alpha1); X52=zeros(1,Nc_dtr*Nt+1); Y52=zeros(1,Nc_dtr*Nt+1); syms v for v=2:Nc_dtr*Nt+1 X52(1,1)=X42(1,Na*Nt+1); Y52(1,1)=Y42(1,Na*Nt+1); X52(1,v)=A2_dtr1(1,v-1)*(X52(1,v-1)-X42(1,1))+B2_dtr1(1,v-1)*(Y_tam-Y52(1,v1))+X42(1,1); Y52(1,v)=Y_tam-A2_dtr1(1,v-1)*(Y_tam-Y52(1,v-1))+B2_dtr1(1,v-1)*(X52(1,v-1)X42(1,1)); end %dec A2_dtr2=cos(b2_alpha2); B2_dtr2=sin(b2_alpha2); X62=zeros(1,Nd*Nt+1); Y62=zeros(1,Nd*Nt+1); syms m for m=2:Nd*Nt+1 X62(1,1)=X52(1,Nc_dtr*Nt+1); Y62(1,1)=Y52(1,Nc_dtr*Nt+1); X62(1,m)=A2_dtr2(1,m-1)*(X62(1,m-1)-X42(1,1))+B2_dtr2(1,m-1)*(Y_tam-Y62(1,m1))+X42(1,1); Y62(1,m)=Y_tam-A2_dtr2(1,m-1)*(Y_tam-Y62(1,m-1))+B2_dtr2(1,m-1)*(X62(1,m-1)X42(1,1)); end %% X12s=X12(:, 1:2500);X21s=X21(:, 1:5500);X32s=X32(:, 1:2500); Y12s=Y12(:, 1:2500);Y21s=Y21(:, 1:5500);Y32s=Y32(:, 1:2500); X42s=X42(:, 1:2500);X52s=X52(:, 1:10060);X62s=X62(:, 1:2500); Y42s=Y42(:, 1:2500);Y52s=Y52(:, 1:10060);Y62s=Y62(:, 1:2500); X72s=X72(:, 1:2500);X82s=X82(:, 1:10060);X92s=X92(:, 1:2501); Y72s=Y72(:, 1:2500);Y82s=Y82(:, 1:10060);Y92s=Y92(:, 1:2501); wave1.time = [];%iput simulink sum=[X42s,X52s,X62s,X12s,X21s,X32s,X72s,X82s,X92s;Y42s,Y52s,Y62s,Y12s,Y21s,Y32s,Y72s,Y 82s,Y92s]'; wave1.signals.values = sum; wave1.signals.dimensions =2; xsml=x_sml; ysml=y_sml; 7.3 Chương trình nội suy chữ C (m-file) clear sum F=5; %toc an dao 50mm/s^2 A=2; %gia toc tang toc cho phep D=2; %gia toc giam toc cho phep L=40; %chieu dai T=5e-3; %chu ky noi suy Tdk=1e-3; Nt=T/Tdk; I=(F*F)/(2*A)+(F*F)/(2*D)-L; %%tinh toan thoi gian tang toc giam toc Ta=F/A;%time acc Td=F/D;%time dec Sa=(F*F)/(2*A); Sd=(F*F)/(2*D); Tc=(L-Sa-Sd)/F;%time cons %%Noi suy 53 BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co Va0=0; Vd0=F; Na=Ta/T; Nd=Td/T; Nc=Tc/T; Va=Va0:T*A:F;%Vi+1=Vi+A*T Vd=Vd0:-T*D:0;%Vi+1=Vi-A*T Vc=ones(1,Nc+1)*F; ta=0:T:Ta; td=Tc+Ta:T:(Tc+Ta+Td); tc=Ta:T:(Ta+Tc); %noi suy duong tron X_tam=0; Y_tam=20; R=20; Wa=Va/R;%w=v/r toc tang toc alpha=Wa*T;%noi suy goc alpha tang toc tong=sum(alpha);%tong quang duong tang toc Wd=Vd/R;%toc giam toc alpha2=Wd*T;%noi suy goc alpha giam toc tong2=sum(alpha2);%tong quang duong giam toc Wc=F/R;%toc cons tong1=pi-tong-tong2;%quang duong cons Tc_dtr=tong1/Wc;%Thoi gian cons Nc_dtr=round(Tc_dtr/T);% so lan noi suy cons Vc_dtr=ones(1,Nc_dtr+1)*F;%toc cons tc_dtr=Ta:T:(Ta+Tc_dtr); Wc_dtr=Vc_dtr/R;%w=v/r alpha1=Wc_dtr*T;% noi suy goc alpha cons A_dtr=cos(alpha);%A=cos B_dtr=sin(alpha);%B=sin X4=zeros(1,Na+1); Y4=zeros(1,Na+1); syms z for z=2:Na+1 X4(1,1)=20;%Hoanh diem A Y4(1,1)=20;%Tung diem A X4(1,z)=X4(1,z-1)*A_dtr(1,z-1)-(Y4(1,z-1)-Y_tam)*B_dtr(1,z-1); Y4(1,z)=(Y4(1,z-1)-Y_tam)*A_dtr(1,z-1)+X4(1,z-1)*B_dtr(1,z-1)+Y_tam; end A_dtr1=cos(alpha1); B_dtr1=sin(alpha1); X5=zeros(1,Nc_dtr+1); Y5=zeros(1,Nc_dtr+1); syms v for v=2:Nc_dtr+1 X5(1,1)=X4(1,Na+1)*A_dtr1(1,v-1)-(Y4(1,Na+1)-Y_tam)*B_dtr1(1,v-1); Y5(1,1)=(Y4(1,Na+1)-Y_tam)*A_dtr1(1,v-1)+X4(1,Na+1)*B_dtr1(1,v-1)+Y_tam; X5(1,v)=X5(1,v-1)*A_dtr1(1,v-1)-(Y5(1,v-1)-Y_tam)*B_dtr1(1,v-1); Y5(1,v)=(Y5(1,v-1)-Y_tam)*A_dtr1(1,v-1)+X5(1,v-1)*B_dtr1(1,v-1)+Y_tam; end A_dtr2=cos(alpha2); B_dtr2=sin(alpha2); X6=zeros(1,Nd+1); Y6=zeros(1,Nd+1); X6_dtr=zeros(1,Nd+1); Y6_dtr=zeros(1,Nd+1); syms m for m=2:Nd+1 54 BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co X6(1,1)=X5(1,Nc_dtr+1); Y6(1,1)=Y5(1,Nc_dtr+1); X6(1,m)=X6(1,m-1)*A_dtr2(1,m-1)-(Y6(1,m-1)-Y_tam)*B_dtr2(1,m-1); Y6(1,m)=(Y6(1,m-1)-Y_tam)*A_dtr2(1,m-1)+X6(1,m-1)*B_dtr2(1,m-1)+Y_tam; end %%tinh toan thoi gian tang toc giam toc Ta=F/A;%time acc Td=F/D;%time dec Sa=(F*F)/(2*A); Sd=(F*F)/(2*D); Tc=(L-Sa-Sd)/F;%time cons %%Noi suy Va0=0; Vd0=F; Na=Ta/T; Nd=Td/T; Nc=Tc/T; Va=Va0:T*A:F;%Vi+1=Vi+A*T Vd=Vd0:-T*D:0;%Vi+1=Vi-A*T Vc=ones(1,Nc+1)*F; ta=0:T:Ta; td=Tc+Ta:T:(Tc+Ta+Td); tc=Ta:T:(Ta+Tc); %tinh toan khoang tang toc deltaL1=Va*T; deltaX1=deltaL1*0; deltaY1=deltaL1*1; syms i X1=zeros(1,Na+1); Y1=zeros(1,Na+1); for i=2:Na+1 X1(1,1)=X6(1,Nd+1); X1(1,i)=X1(1,i-1)-deltaX1(1,i); Y1(1,1)=Y6(1,Nd+1); Y1(1,i)=Y1(1,i-1)-deltaY1(1,i); end %tinh toan khoang cons deltaL2=Vc*T; deltaX2=deltaL2*0; deltaY2=deltaL2*1; syms j X2=zeros(1,Nc+1); Y2=zeros(1,Nc+1); for j=2:Nc+1 X2(1,1)=X1(1,Na+1); X2(1,j)=X2(1,j-1)-deltaX2(1,j); Y2(1,1)=Y1(1,Na+1); Y2(1,j)=Y2(1,j-1)-deltaY2(1,j); end %tinh toan khoang giam toc deltaL3=Vd*T; deltaX3=deltaL3*0; deltaY3=deltaL3*1; syms k X3=zeros(1,Nd); Y3=zeros(1,Nd); for k=2:Nd+1 X3(1,1)=X2(1,Nc+1); X3(1,k)=X3(1,k-1)-deltaX3(1,k); Y3(1,1)=Y2(1,Nc+1); Y3(1,k)=Y3(1,k-1)-deltaY3(1,k); end %noi suy duong tron 55 BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co X_tam1=0; Y_tam1=-20; R=20; Wa=Va/R;%w=v/r toc tang toc alpha=Wa*T;%noi suy goc alpha tang toc tong=sum(alpha);%tong quang duong tang toc Wd=Vd/R;%toc giam toc alpha2=Wd*T;%noi suy goc alpha giam toc tong2=sum(alpha2);%tong quang duong giam toc Wc=F/R;%toc cons tong1=pi-tong-tong2;%quang duong cons Tc_dtr=tong1/Wc;%Thoi gian cons Nc_dtr=round(Tc_dtr/T);% so lan noi suy cons Vc_dtr=ones(1,Nc_dtr+1)*F;%toc cons tc_dtr=[Ta:T:(Ta+Tc_dtr)];% ko de lam gi :) Wc_dtr=Vc_dtr/R;%w=v/r alpha1=Wc_dtr*T;% noi suy goc alpha cons td_dtr=Tc_dtr+Ta:T:(Tc_dtr+Ta+Td); A_dtr=cos(alpha);%A=cos B_dtr=sin(alpha);%B=sin X7=zeros(1,Na+1); Y7=zeros(1,Na+1); syms q for q=2:Na+1 X7(1,1)=X3(1,Nd+1);%Hoanh diem A Y7(1,1)=Y3(1,Nd+1);%Tung diem A X7(1,q)=X7(1,q-1)*A_dtr(1,q-1)-(Y7(1,q-1)-Y_tam1)*B_dtr(1,q-1); Y7(1,q)=(Y7(1,q-1)-Y_tam1)*A_dtr(1,q-1)+X7(1,q-1)*B_dtr(1,q-1)+Y_tam1; end A_dtr1=cos(alpha1); B_dtr1=sin(alpha1); X8=zeros(1,Nc_dtr+1); Y8=zeros(1,Nc_dtr+1); syms p for p=2:Nc_dtr+1 X8(1,1)=X7(1,Na+1); Y8(1,1)=Y7(1,Na+1); X8(1,p)=X8(1,p-1)*A_dtr1(1,p-1)-(Y8(1,p-1)-Y_tam1)*B_dtr1(1,p-1); Y8(1,p)=(Y8(1,p-1)-Y_tam1)*A_dtr1(1,p-1)+X8(1,p-1)*B_dtr1(1,p-1)+Y_tam1; end A_dtr2=cos(alpha2); B_dtr2=sin(alpha2); X9=zeros(1,Nd+1); Y9=zeros(1,Nd+1); for l=2:Nd+1 X9(1,1)=X8(1,Nc_dtr+1)*A_dtr2(1,l-1)-(Y8(1,Nc_dtr+1)-Y_tam1)*B_dtr2(1,l-1); Y9(1,1)=(Y8(1,Nc_dtr+1)-Y_tam1)*A_dtr2(1,l-1)+X8(1,Nc_dtr+1)*B_dtr2(1,l-1)+Y_tam1; X9(1,l)=X9(1,l-1)*A_dtr2(1,l-1)-(Y9(1,l-1)-Y_tam1)*B_dtr2(1,l-1); Y9(1,l)=(Y9(1,l-1)-Y_tam1)*A_dtr2(1,l-1)+X9(1,l-1)*B_dtr2(1,l-1)+Y_tam1; end %% %noi suy tinh duong thang %tinh toan khoang tang toc ns tinh syms i j%ns tuyen tinh aX1=zeros(1,Na*Nt+1);aX1(1,Na*Nt+1)=deltaX1(1,Na+1)/Nt; aY1=zeros(1,Na*Nt+1);aY1(1,Na*Nt+1)=deltaY1(1,Na+1)/Nt; for i=1:Na for j=(Nt*i-Nt+1):(Nt*i) aX1(1,j)=deltaX1(1,i)/Nt; aY1(1,j)=deltaY1(1,i)/Nt; end 56 BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co end syms k %ns trung binh bX1=zeros(1,Na*Nt+1);bY1=zeros(1,Na*Nt+1); b1X1=zeros(1,Na*Nt+1);b1Y1=zeros(1,Na*Nt+1); b2X1=zeros(1,Na*Nt+1);b2Y1=zeros(1,Na*Nt+1); for k=2:Na*Nt-3 bX1(1,k)=(aX1(1,k+3)+aX1(1,k+2)+aX1(1,k+1)+aX1(1,k)+aX1(1,k-1))/Nt; b1X1(1,k)=(aX1(1,k+4)+aX1(1,k+3)+aX1(1,k+2)+aX1(1,k+1)+aX1(1,k))/Nt; b2X1(1,k)=(bX1(1,k)+b1X1(1,k))/2; bY1(1,k)=(aY1(1,k+3)+aY1(1,k+2)+aY1(1,k+1)+aY1(1,k)+aY1(1,k-1))/Nt; b1Y1(1,k)=(aY1(1,k+4)+aY1(1,k+3)+aY1(1,k+2)+aY1(1,k+1)+aY1(1,k))/Nt; b2Y1(1,k)=(bY1(1,k)+b1Y1(1,k))/2; end for k=Na*Nt-2:Na*Nt+1 bX1(1,k)=((k-Na*Nt+3)*aX1(1,Na*Nt+1)+(Nt-k+Na*Nt-3)*aX1(1,k))/Nt; b1X1(1,k)=((k-Na*Nt+4)*aX1(1,Na*Nt+1)+(Nt-k+Na*Nt-4)*aX1(1,k))/Nt; b2X1(1,k)=(bX1(1,k)+b1X1(1,k))/2; bY1(1,k)=((k-Na*Nt+3)*aY1(1,Na*Nt+1)+(Nt-k+Na*Nt-3)*aY1(1,k))/Nt; b1Y1(1,k)=((k-Na*Nt+4)*aY1(1,Na*Nt+1)+(Nt-k+Na*Nt-4)*aY1(1,k))/Nt; b2Y1(1,k)=(bY1(1,k)+b1Y1(1,k))/2; end syms l X11=zeros(1,Na*Nt+1);X12=zeros(1,Na*Nt+1); Y11=zeros(1,Na*Nt+1);Y12=zeros(1,Na*Nt+1); for l=2:Na*Nt+1 e=0.0319; X11(1,1)=-20;X12(1,1)=-20; X11(1,l)=X11(1,l-1)-aX1(1,l);X12(1,l)=X12(1,l-1)-b2X1(1,l); Y11(1,1)=20+e;Y12(1,1)=20+e; Y11(1,l)=Y11(1,l-1)-aY1(1,l);Y12(1,l)=Y12(1,l-1)-b2Y1(1,l); end %tinh toan khoang cons ns tinh syms i j k aX2=zeros(1,Nc*Nt+1);aX2(1,Nc*Nt+1)=deltaX2(1,Nc+1)/Nt; aY2=zeros(1,Nc*Nt+1);aY2(1,Nc*Nt+1)=deltaY2(1,Nc+1)/Nt; X21=zeros(1,Nc*Nt+1);Y21=zeros(1,Nc*Nt+1); for i=1:Nc for j=(Nt*i-Nt+1):(Nt*i) aX2(1,j)=deltaX2(1,i)/Nt; aY2(1,j)=deltaY2(1,i)/Nt; end end for k=2:Nc*Nt+1 X21(1,1)=X11(1,Na*Nt+1); X21(1,k)=X21(1,k-1)-aX2(1,k); Y21(1,1)=Y11(1,Na*Nt+1); Y21(1,k)=Y21(1,k-1)-aY2(1,k); end %tinh toan khoang giam toc ns tinh syms i j%ns tuyen tinh aX3=zeros(1,Nd*Nt+1); aX3(1,Nd*Nt+1)=deltaX3(1,Nd+1)/Nt; aY3=zeros(1,Nd*Nt+1); aY3(1,Nd*Nt+1)=deltaY3(1,Nd+1)/Nt; for i=1:Nd for j=(Nt*i-Nt+1):(Nt*i) aX3(1,j)=deltaX3(1,i)/Nt; aY3(1,j)=deltaY3(1,i)/Nt; end end syms k %ns trung binh bX3=zeros(1,Nd*Nt+1);bY3=zeros(1,Nd*Nt+1); b1X3=zeros(1,Nd*Nt+1);b1Y3=zeros(1,Nd*Nt+1); b2X3=zeros(1,Nd*Nt+1);b2Y3=zeros(1,Nd*Nt+1); for k=1:4 bX3(1,k)=((5-k)*aX3(1,1)+(k-1)*aX3(1,k))/Nt; b1X3(1,k)=((6-k)*aX3(1,1)+(k)*aX3(1,k))/Nt; b2X3(1,k)=(bX3(1,k)+b1X3(1,k))/2; 57 BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co bY3(1,k)=((5-k)*aY3(1,1)+(k-1)*aY3(1,k))/Nt; b1Y3(1,k)=((6-k)*aY3(1,1)+(k)*aY3(1,k))/Nt; b2Y3(1,k)=(bY3(1,k)+b1Y3(1,k))/2; end for k=5:Nd*Nt bX3(1,k)=(aX3(1,k)+aX3(1,k-1)+aX3(1,k-2)+aX3(1,k-3)+aX3(1,k-4))/Nt; b1X3(1,k)=(aX3(1,k+1)+aX3(1,k)+aX3(1,k-1)+aX3(1,k-2)+aX3(1,k-3))/Nt; b2X3(1,k)=(bX3(1,k)+b1X3(1,k))/2; bY3(1,k)=(aY3(1,k)+aY3(1,k-1)+aY3(1,k-2)+aY3(1,k-3)+aY3(1,k-4))/Nt; b1Y3(1,k)=(aY3(1,k+1)+aY3(1,k)+aY3(1,k-1)+aY3(1,k-2)+aY3(1,k-3))/Nt; b2Y3(1,k)=(bY3(1,k)+b1Y3(1,k))/2; end syms l X31=zeros(1,Nd*Nt+1);X32=zeros(1,Nd*Nt+1); Y31=zeros(1,Nd*Nt+1);Y32=zeros(1,Nd*Nt+1); for l=2:Nd*Nt+1 X31(1,1)=X21(1,Nc*Nt+1);X32(1,1)=X21(1,Nc*Nt+1); X31(1,l)=X31(1,l-1)-aX3(1,l);X32(1,l)=X32(1,l-1)-b2X3(1,l); Y31(1,1)=Y21(1,Nc*Nt+1);Y32(1,1)=Y21(1,Nc*Nt+1); Y31(1,l)=Y31(1,l-1)-aY3(1,l);Y32(1,l)=Y32(1,l-1)-b2Y3(1,l); end syms l %% %noi suy tinh duong tron %noi suy tinh acc syms i j%ns tuyen tinh a_alpha=zeros(1,Na*Nt+1);a_alpha(1,Na*Nt+1)=alpha(1,Na+1)/Nt; for i=1:Na for j=(Nt*i-Nt+1):(Nt*i) a_alpha(1,j)=alpha(1,i)/Nt; end end syms k %ns trung binh b_alpha=zeros(1,Na*Nt+1); b1_alpha=zeros(1,Na*Nt+1); b2_alpha=zeros(1,Na*Nt+1); for k=2:Na*Nt-3 b_alpha(1,k)=(a_alpha(1,k+3)+a_alpha(1,k+2)+a_alpha(1,k+1)+a_alpha(1,k)+a_alpha(1,k1))/Nt; b1_alpha(1,k)=(a_alpha(1,k+4)+a_alpha(1,k+3)+a_alpha(1,k+2)+a_alpha(1,k+1)+a_alpha(1,k ))/Nt; b2_alpha(1,k)=(b_alpha(1,k)+b1_alpha(1,k))/2; end for k=Na*Nt-2:Na*Nt+1 b_alpha(1,k)=((k-Na*Nt+3)*a_alpha(1,Na*Nt+1)+(Nt-k+Na*Nt-3)*a_alpha(1,k))/Nt; b1_alpha(1,k)=((k-Na*Nt+4)*a_alpha(1,Na*Nt+1)+(Nt-k+Na*Nt-4)*a_alpha(1,k))/Nt; b2_alpha(1,k)=(b_alpha(1,k)+b1_alpha(1,k))/2; end %noi suy tinh cons syms i j k a_alpha1=zeros(1,Nc_dtr*Nt+1);a_alpha1(1,Nc_dtr*Nt+1)=alpha1(1,Nc_dtr+1)/Nt; for i=1:Nc_dtr for j=(Nt*i-Nt+1):(Nt*i) a_alpha1(1,j)=alpha1(1,i)/Nt; end end %noi suy tinh dec syms i j%ns tuyen tinh a_alpha2=zeros(1,Nd*Nt+1);a_alpha2(1,Nd*Nt+1)=alpha2(1,Nd+1)/Nt; for i=1:Nd for j=(Nt*i-Nt+1):(Nt*i) a_alpha2(1,j)=alpha2(1,i)/Nt; end end 58 BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co syms k %ns trung binh b_alpha2=zeros(1,Nd*Nt+1); b1_alpha2=zeros(1,Nd*Nt+1); b2_alpha2=zeros(1,Nd*Nt+1); for k=1:4 b_alpha2(1,k)=((5-k)*a_alpha2(1,1)+(k-1)*a_alpha2(1,k))/Nt; b1_alpha2(1,k)=((6-k)*a_alpha2(1,1)+(k)*a_alpha2(1,k))/Nt; b2_alpha2(1,k)=(b_alpha2(1,k)+b1_alpha2(1,k))/2; end for k=5:Nd*Nt b_alpha2(1,k)=(a_alpha2(1,k)+a_alpha2(1,k-1)+a_alpha2(1,k-2)+a_alpha2(1,k3)+a_alpha2(1,k-4))/Nt; b1_alpha2(1,k)=(a_alpha2(1,k+1)+a_alpha2(1,k)+a_alpha2(1,k-1)+a_alpha2(1,k2)+a_alpha2(1,k-3))/Nt; b2_alpha2(1,k)=(b_alpha2(1,k)+b1_alpha2(1,k))/2; end %tinh toan dich chuyen ns tinh tuyen tinh %acc A1_dtr=cos(a_alpha);%A=cos B1_dtr=sin(a_alpha);%B=sin X41=zeros(1,Na*Nt+1);X71=zeros(1,Na*Nt+1); Y41=zeros(1,Na*Nt+1);Y71=zeros(1,Na*Nt+1); syms z for z=2:Na*Nt+1 X41(1,1)=20;X71(1,1)=-20;%Hoanh diem A Y41(1,1)=20;Y71(1,1)=-20+e;%Tung diem A X41(1,z)=X41(1,z-1)*A1_dtr(1,z-1)-(Y41(1,z-1)-Y_tam)*B1_dtr(1,z-1); Y41(1,z)=(Y41(1,z-1)-Y_tam)*A1_dtr(1,z-1)+X41(1,z-1)*B1_dtr(1,z-1)+Y_tam; X71(1,z)=X71(1,z-1)*A1_dtr(1,z-1)-(Y71(1,z-1)-Y_tam1)*B1_dtr(1,z-1); Y71(1,z)=(Y71(1,z-1)-Y_tam1)*A1_dtr(1,z-1)+X71(1,z-1)*B1_dtr(1,z-1)+Y_tam1; end %cons A1_dtr1=cos(a_alpha1); B1_dtr1=sin(a_alpha1); X51=zeros(1,Nc_dtr*Nt+1);X81=zeros(1,Nc_dtr*Nt+1); Y51=zeros(1,Nc_dtr*Nt+1);Y81=zeros(1,Nc_dtr*Nt+1); syms v for v=2:Nc_dtr*Nt+1 X51(1,1)=X41(1,Na*Nt+1);X81(1,1)=X71(1,Na*Nt+1); Y51(1,1)=Y41(1,Na*Nt+1);Y81(1,1)=Y71(1,Na*Nt+1); X51(1,v)=X51(1,v-1)*A1_dtr1(1,v-1)-(Y51(1,v-1)-Y_tam)*B1_dtr1(1,v-1); Y51(1,v)=(Y51(1,v-1)-Y_tam)*A1_dtr1(1,v-1)+X51(1,v-1)*B1_dtr1(1,v-1)+Y_tam; X81(1,v)=X81(1,v-1)*A1_dtr1(1,v-1)-(Y81(1,v-1)-Y_tam1)*B1_dtr1(1,v-1); Y81(1,v)=(Y81(1,v-1)-Y_tam1)*A1_dtr1(1,v-1)+X81(1,v-1)*B1_dtr1(1,v-1)+Y_tam1; end %dec A1_dtr2=cos(a_alpha2); B1_dtr2=sin(a_alpha2); X61=zeros(1,Nd*Nt+1);X91=zeros(1,Nd*Nt+1); Y61=zeros(1,Nd*Nt+1);Y91=zeros(1,Nd*Nt+1); syms m for m=2:Nd*Nt+1 X61(1,1)=X51(1,Nc_dtr*Nt+1);X91(1,1)=X81(1,Nc_dtr*Nt+1); Y61(1,1)=Y51(1,Nc_dtr*Nt+1);Y91(1,1)=Y81(1,Nc_dtr*Nt+1); X61(1,m)=X61(1,m-1)*A1_dtr2(1,m-1)-(Y61(1,m-1)-Y_tam)*B1_dtr2(1,m-1); Y61(1,m)=(Y61(1,m-1)-Y_tam)*A1_dtr2(1,m-1)+X61(1,m-1)*B1_dtr2(1,m-1)+Y_tam; X91(1,m)=X91(1,m-1)*A1_dtr2(1,m-1)-(Y91(1,m-1)-Y_tam1)*B1_dtr2(1,m-1); Y91(1,m)=(Y91(1,m-1)-Y_tam1)*A1_dtr2(1,m-1)+X91(1,m-1)*B1_dtr2(1,m-1)+Y_tam1; end %% %tinh toan dich chuyen ns tinh trung binh %acc A2_dtr=cos(b2_alpha);%A=cos B2_dtr=sin(b2_alpha);%B=sin X42=zeros(1,Na*Nt+1);X72=zeros(1,Na*Nt+1); Y42=zeros(1,Na*Nt+1);Y72=zeros(1,Na*Nt+1); 59 BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co syms z for z=2:Na*Nt+1 X42(1,1)=20;X72(1,1)=-20;%Hoanh diem A Y42(1,1)=20;Y72(1,1)=-20+e;%Tung diem A X42(1,z)=X42(1,z-1)*A2_dtr(1,z-1)-(Y42(1,z-1)-Y_tam)*B2_dtr(1,z-1); Y42(1,z)=(Y42(1,z-1)-Y_tam)*A2_dtr(1,z-1)+X42(1,z-1)*B2_dtr(1,z-1)+Y_tam; X72(1,z)=X72(1,z-1)*A2_dtr(1,z-1)-(Y72(1,z-1)-Y_tam1)*B2_dtr(1,z-1); Y72(1,z)=(Y72(1,z-1)-Y_tam1)*A2_dtr(1,z-1)+X72(1,z-1)*B2_dtr(1,z-1)+Y_tam1; end %cons A2_dtr1=cos(a_alpha1); B2_dtr1=sin(a_alpha1); X52=zeros(1,Nc_dtr*Nt+1);X82=zeros(1,Nc_dtr*Nt+1); Y52=zeros(1,Nc_dtr*Nt+1);Y82=zeros(1,Nc_dtr*Nt+1); syms v for v=2:Nc_dtr*Nt+1 X52(1,1)=X42(1,Na*Nt+1);X82(1,1)=X72(1,Na*Nt+1); Y52(1,1)=Y42(1,Na*Nt+1);Y82(1,1)=Y72(1,Na*Nt+1); X52(1,v)=X52(1,v-1)*A2_dtr1(1,v-1)-(Y52(1,v-1)-Y_tam)*B2_dtr1(1,v-1); Y52(1,v)=(Y52(1,v-1)-Y_tam)*A2_dtr1(1,v-1)+X52(1,v-1)*B2_dtr1(1,v-1)+Y_tam; X82(1,v)=X82(1,v-1)*A2_dtr1(1,v-1)-(Y82(1,v-1)-Y_tam1)*B2_dtr1(1,v-1); Y82(1,v)=(Y82(1,v-1)-Y_tam1)*A2_dtr1(1,v-1)+X82(1,v-1)*B2_dtr1(1,v-1)+Y_tam1; end %dec A2_dtr2=cos(b2_alpha2); B2_dtr2=sin(b2_alpha2); X62=zeros(1,Nd*Nt+1);X92=zeros(1,Nd*Nt+1); Y62=zeros(1,Nd*Nt+1);Y92=zeros(1,Nd*Nt+1); syms m for m=2:Nd*Nt+1 X62(1,1)=X52(1,Nc_dtr*Nt+1);X92(1,1)=X82(1,Nc_dtr*Nt+1); Y62(1,1)=Y52(1,Nc_dtr*Nt+1);Y92(1,1)=Y82(1,Nc_dtr*Nt+1); X62(1,m)=X62(1,m-1)*A2_dtr2(1,m-1)-(Y62(1,m-1)-Y_tam)*B2_dtr2(1,m-1); Y62(1,m)=(Y62(1,m-1)-Y_tam)*A2_dtr2(1,m-1)+X62(1,m-1)*B2_dtr2(1,m-1)+Y_tam; X92(1,m)=X92(1,m-1)*A2_dtr2(1,m-1)-(Y92(1,m-1)-Y_tam1)*B2_dtr2(1,m-1); Y92(1,m)=(Y92(1,m-1)-Y_tam1)*A2_dtr2(1,m-1)+X92(1,m-1)*B2_dtr2(1,m-1)+Y_tam1; end %% X1s=X11(:, 1:2500);X2s=X21(:, 1:5500);X3s=X31(:, 1:2501); Y1s=Y11(:, 1:2500);Y2s=Y21(:, 1:5500);Y3s=Y31(:, 1:2501); X4s=X41(:, 1:2500);X5s=X51(:, 1:10060);X6s=X61(:, 1:2501); Y4s=Y41(:, 1:2500);Y5s=Y51(:, 1:10060);Y6s=Y61(:, 1:2501); X7s=X71(:, 1:2500);X8s=X81(:, 1:10060);X9s=X91(:, 1:2501); Y7s=Y71(:, 1:2500);Y8s=Y81(:, 1:10060);Y9s=Y91(:, 1:2501); X10s=zeros(1,15001-13232); Y10s=zeros(1,15001-13232); for t=1:(30001-13232) X10s(1,t)=X9s(1,Nd*Nt+1); Y10s(1,t)=Y9s(1,Nd*Nt+1); end wave1.time = [];%iput simulink sum=[X4s,X5s,X6s,X1s,X2s,X3s,X7s,X8s,X9s,X10s;Y4s,Y5s,Y6s,Y1s,Y2s,Y3s,Y7s,Y8s,Y9s,Y10s ]'; wave1.signals.values = sum; wave1.signals.dimensions =2; xsml=x_sml; ysml=y_sml; 7.4 Nội suy thô đường thẳng với kiểu tăng/giảm tốc sau nội suy dạng hình thang %%thong so F=5; %toc an dao 50mm/s^2 A=2; %gia toc tang toc cho phep D=2; %gia toc giam toc cho phep L=50; %chieu dai T=5e-3; %chu ky noi suy %%tinh toan thoi gian tang toc giam toc 60 BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co Ta=F/A;%time acc Na=Ta/T;%Pulse acc Tsum=L/F;%total time %%tinh toan khoang dich chuyen dX=L*0.8; dY=L*0.6; N=Tsum/T;%Pulse sum syms i deltaX=zeros(1,N+1);%khoang dich chuyen truc X deltaY=zeros(1,N+1);%khoang dich chuyen truc Y for i=1:N deltaX(i)=dX/N; deltaY(i)=dY/N; end %%output XLO=zeros(1,N+Na); YLO=zeros(1,N+Na); for i=2:Na%acc XLO(1)=deltaX(1)/Na;YLO(1)=deltaY(1)/Na; XLO(i)=deltaX(i)/Na+XLO(i-1); YLO(i)=deltaY(i)/Na+YLO(i-1); end for i=Na+1:N%cons XLO(i)=(deltaX(i)-deltaX(i-Na))/Na+XLO(i-1); YLO(i)=(deltaY(i)-deltaY(i-Na))/Na+YLO(i-1); end for i=N+1:N+Na%dec XLO(i)=-deltaX(i-Na)/Na+XLO(i-1); YLO(i)=-deltaY(i-Na)/Na+YLO(i-1); end X=zeros(1,N+Na); Y=zeros(1,N+Na); for i=2:N+Na X(1,1)=10; X(1,i)=X(1,i-1)+XLO(1,i); Y(1,1)=10; Y(1,i)=Y(1,i-1)+YLO(1,i); End 7.5 Nội suy thô đường trịn với kiểu tăng/giảm tốc sau nội suy dạng hình thang %%thong so F=5; %toc an dao 50mm/s^2 A=2; %gia toc tang toc cho phep D=2; %gia toc giam toc cho phep alpha=1.287;%tong cung dich chuyen T=5e-3; %chu ky noi suy X_tam=100/10;%tam duong tron truc X Y_tam=(310/6);%tam duong tron truc Y R=1250/30;%ban kinh %%tinh toan thoi gian tang toc giam toc Ta=F/A;%time acc Na=Ta/T;%Pulse acc Tsum=10.72;%total time %%tinh toan khoang dich chuyen N=Tsum/T;%Pulse sum syms i d_alpha=zeros(1,N+1);%khoang dich chuyen goc alpha for i=1:N d_alpha(i)=alpha/N; end %%output alphaLO=zeros(1,N+Na); for i=2:Na%acc alphaLO(1)=d_alpha(1)/Na; alphaLO(i)=d_alpha(i)/Na+alphaLO(i-1); 61 BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co BAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.coBAI.tap.lon.mon.he.THONG.dieu.KHIEN.may.CNC.may.CNC.hai.truc.x y.moi.truc.co.1.dong.co
Ngày đăng: 20/12/2023, 22:30
Xem thêm: