BÀI tập lớn môn hệ THỐNG điều KHIỂN máy CNC máy CNC hai trục x, y mỗi trục có 1 động cơ

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 hiện (Nhóm 5): 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 1 TIEU LUAN MOI download : skknchat123@gmail.com MỤC LỤC 1 YÊU CẦU BÀI TOÁN VÀ LỰA CHỌN THAM SỐ 2 THIẾT KẾ BỘ ĐIỀU KHIỂN VỊ TRÍ PID CHO 2 TRỤC ĐỘNG CƠ 2.1 Các bước thiết kế bộ điều khiển 2.2 Thiết kế bộ điều khiển 2.2.1 Trục x 2.2.2 Trục y 3 NỘI SUY ĐƯỜNG THẲNG 3.1 Cấu trúc bộ điều khiển trung tâm 3.2 Look Ahead 3.3 Tính toá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 3.5 Nội suy lệnh đặt cho đường thẳng (trích mẫu dữ liệu) 3.6 Nội suy tinh 3.6.1 Nội suy tuyến tính 3.6.2 Nội suy trung bình 3.6.3 Quỹ đạo sau nội suy tinh 3.7 Thực hiện điều khiển vị trí 4 NỘI SUY ĐƯỜNG TRÒN 4.1 Tính toán thời gian tăng tốc, giảm tốc, vận tốc không đổi 4.2 Nội suy vận tốc 4.3 Nội suy sai lệch đặt cho đường tròn 4.4 Nội suy tinh 5 THỰC HIỆN VẼ CHỮ “C” 6 NỘI SUY THÔ QUỸ ĐẠO VỚI TĂNG/GIẢM TỐC SAU NỘI SUY 6.1 Nội suy tuyến tính 2 TIEU LUAN MOI download : skknchat123@gmail.com 6.2 Nội suy đường tròn 42 7 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 trên simulink 62 3 TIEU LUAN MOI download : skknchat123@gmail.com 1.YÊU CẦU BÀI TOÁN VÀ LỰA CHỌN THAM SỐ 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 Giả sử hàm truyền với đầu vào là điện áp điều khiển, đầu ra là tốc độ của 2 hệ truyền động 2 trục X và Y là khâu quán tính bậc nhất 1 Tự chọn thông số 02 hàm truyền trên và thiết kế bộ điều khiển vị trí cho từng trục thỏa mãn các yêu cầu sau: - Độ quá điều chỉnh ≤ 5% - Sai lệch tĩnh ≤ 0,1% - Thời gian xác lập nhỏ nhất - | Uđk | ≤ 10V Nhóm em lựa chọn thông số 2 hàm truyền như sau: Trục x: • Trục y: • 2 Thực hiện nội suy đường thẳng và đườ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 hiện cả nội suy thô và 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ô là 5ms, nội suy tinh là 1ms 3 Thực hiện mô phỏng hệ thống di chuyển theo quỹ đạo hình chữ O hoặc C 4 Trình bày và nêu ví dụ minh họa (thông số tùy chọn) về nội suy thô đường thẳng và đườ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ể sẽ trình bày cụ thể trong các phần tiếp theo 4 TIEU LUAN MOI download : skknchat123@gmail.com 2 THIẾT KẾ BỘ ĐIỀU KHIỂN VỊ TRÍ PID CHO 2 TRỤC ĐỘNG CƠ Với thông số 2 hàm truyền động cơ: • Trục x: • Trục y: Chọn phương pháp thiết kế bộ điều khiển vị trí: Vì 2 hàm truyền trên đều có khâu tích phân trong hàm truyền vì thế sẽ loại bỏ được sai lệch tĩnh Bởi vậy, bộ điều khiển vị trí được chọn sẽ là bộ PD với hàm truyền: ( )= (1+ ) 2.1 Các bước thiết kế bộ điều khiển - B1: Đặt giá trị Setpoint là tín hiệu Step, biên độ là 1 - B2: Đặt giá trị , = =0 - B3: Quan sát đáp ứng đầu ra của 2 trục và quan sát , sau khi thỏa mãn giá trị cho phép thì tinh chỉnh hệ số của bộ điều khiển đạt thỏa mãn yêu cầu về thời gian xác lập và độ quá điều chỉnh Hình 2 1 Mô hình mô phỏng Matlab Simulink 2.2 Thiết kế bộ điều khiển 2.2.1 Trục x Sau khi thực hiện các bước như trên, ta thu được bộ điều khiển vị trí động cơ theo trục x với các thông số: = 10, = 0.005 5 TIEU LUAN MOI download : skknchat123@gmail.com Ta thu được kết quả mô phỏng: Hình 2 2 Đáp ứng vị trí của bộ điều khiển theo trục x Nhận xét: ▪ Đáp ứng đầu ra của động cơ theo trục x bám sát giá trị đặt ▪ Có sai lệch tĩnh xấp xỉ 0 từ thời điểm t = 0.45s ▪ Thời gian xác lập: 0,23s ▪ Độ quá điều chỉnh: 4.1% < 5% 6 TIEU LUAN MOI download : skknchat123@gmail.com Hình 2 3 Điện áp điều khiển trục x → Nhận xét: Điện áp điều khiển < 10V Từ những nhận xét trên ta kết luận: Bộ điều khiển vị trí theo trục x thỏa mãn yêu cầu đề bài 2.2.2 Trục y Sau khi thực hiện các bước như trên, ta thu được bộ điều khiển vị trí động cơ theo trục x với các thông số: = 10, = 0.025 Ta thu được kết quả mô phỏng: 7 TIEU LUAN MOI download : skknchat123@gmail.com Hình 2 4 Đáp ứng vị trí của bộ điều khiển theo trục y Nhận xét: ▪ Đáp ứng đầu ra của động cơ theo trục y bám sát giá trị đặt ▪ ▪ ▪ Có sai lệch tĩnh xấp xỉ 0 từ thời điểm t = 0.45s Thời gian xác lập: 0,24s Độ quá điều chỉnh: 4% < 5% Hình 2 5 Đ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ững nhận xét trên ta kết luận: Bộ điều khiển vị trí theo trục x thỏa mãn yêu cầu đề bài 8 TIEU LUAN MOI download : skknchat123@gmail.com 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 TIEU LUAN MOI download : skknchat123@gmail.com 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 do 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 TIEU LUAN MOI download : skknchat123@gmail.com 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 do tang toc alpha=Wa*T;%noi suy goc alpha khi tang toc tong=sum(alpha);%tong quang duong tang toc Wd=Vd/R;%toc do giam toc alpha2=Wd*T;%noi suy goc alpha khi giam toc tong2=sum(alpha2);%tong quang duong giam toc Wc=F/R;%toc do 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 do 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 TIEU LUAN MOI download : skknchat123@gmail.com 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 do diem A Y4(1,1)=10;%Tung do 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 TIEU LUAN MOI download : skknchat123@gmail.com 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 do diem A Y41(1,1)=10;%Tung do 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 do diem A 52 TIEU LUAN MOI download : skknchat123@gmail.com Y42(1,1)=10;%Tung do 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 do 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 TIEU LUAN MOI download : skknchat123@gmail.com 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 do tang toc alpha=Wa*T;%noi suy goc alpha khi tang toc tong=sum(alpha);%tong quang duong tang toc Wd=Vd/R;%toc do giam toc alpha2=Wd*T;%noi suy goc alpha khi giam toc tong2=sum(alpha2);%tong quang duong giam toc Wc=F/R;%toc do 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 do 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 do diem A Y4(1,1)=20;%Tung do 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 TIEU LUAN MOI download : skknchat123@gmail.com 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); end 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; %%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 TIEU LUAN MOI download : skknchat123@gmail.com X_tam1=0; Y_tam1=-20; R=20; Wa=Va/R;%w=v/r toc do tang toc alpha=Wa*T;%noi suy goc alpha khi tang toc tong=sum(alpha);%tong quang duong tang toc Wd=Vd/R;%toc do giam toc alpha2=Wd*T;%noi suy goc alpha khi giam toc tong2=sum(alpha2);%tong quang duong giam toc Wc=F/R;%toc do 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 do 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 do diem A Y7(1,1)=Y3(1,Nd+1);%Tung do 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,l1)-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 TIEU LUAN MOI download : skknchat123@gmail.com 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 TIEU LUAN MOI download : skknchat123@gmail.com 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 TIEU LUAN MOI download : skknchat123@gmail.com 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 do diem A Y41(1,1)=20;Y71(1,1)=-20+e;%Tung do diem A X41(1,z)=X41(1,z-1)*A1_dtr(1,z1)-(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,z1)*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 TIEU LUAN MOI download : skknchat123@gmail.com syms z for z=2:Na*Nt+1 X42(1,1)=20;X72(1,1)=-20;%Hoanh do diem A Y42(1,1)=20;Y72(1,1)=-20+e;%Tung do diem A X42(1,z)=X42(1,z-1)*A2_dtr(1,z1)-(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,z1)*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 do 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 TIEU LUAN MOI download : skknchat123@gmail.com 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 do 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 TIEU LUAN MOI download : skknchat123@gmail.com end for i=Na+1:N%cons alphaLO(i)=(d_alpha(i)-d_alpha(i-Na))/Na+alphaLO(i-1); end for i=N+1:N+Na%dec alphaLO(i)=-d_alpha(i-Na)/Na+alphaLO(i-1); end A_dtr=cos(alphaLO);%A=cos B_dtr=sin(alphaLO);%B=sin X_SP=zeros(1,N); Y_SP=zeros(1,N); for i=2:N+Na X_SP(1,1)=10;%Hoanh do diem A Y_SP(1,1)=10;%Tung do diem A X_SP(1,i)=A_dtr(1,i-1)*((X_SP(1,i-1))-X_SP(1,1))+B_dtr(1,i-1)*(Y_tam-Y_SP(1,i1))+X_SP(1,1); Y_SP(1,i)=Y_tam-A_dtr(1,i-1)*(Y_tam-Y_SP(1,i-1))+B_dtr(1,i-1)*(X_SP(1,i-1)X_SP(1,1)); end X=zeros(1,N+Na); Y=zeros(1,N+Na); for i=2:N+Na X(1,1)=10;%Hoanh do diem A Y(1,1)=10;%Tung do diem A X(1,i)=A_dtr(1,i-1)*((X(1,i-1))-X(1,1))+B_dtr(1,i-1)*(Y_tam-Y(1,i-1))+X(1,1); Y(1,i)=Y_tam-A_dtr(1,i-1)*(Y_tam-Y(1,i-1))+B_dtr(1,i-1)*(X(1,i-1)-X(1,1)); end 7.6 Chương trình điều khiển trên simulink Đường thẳng + Đường tròn: Chữ C: 62 TIEU LUAN MOI download : skknchat123@gmail.com ... … 24 91 2492 499 2493 2494 15 TIEU LUAN MOI download : skknchat123@gmail.com 2495 2496 2497 2498 2499 2500 500 … 11 9 91 119 92 11 993 11 994 11 995 11 996 11 997 11 998 11 999 12 000 2399 2400 … Từ thu kết... skknchat123@gmail.com 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: Chọn phương pháp thiết kế điều khiển vị trí: Vì hàm truyền có khâu... 61 7.6 Chương trình điều khiển simulink 62 TIEU LUAN MOI download : skknchat123@gmail.com 1 .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