BÁO cáo bài tập lớn hệ THỐNG điều KHIỂN máy CNC một hệ thống CNC gồm 2 trục x và y được điều khiển bởi 2 động cơ

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI VIỆN ĐIỆN CHUYÊN NGÀNH KỸ THUẬT ĐIỀU KHIỂN VÀ TỰ ĐỘNG HÓA BÁO CÁO BÀI TẬP LỚN HỆ THỐNG ĐIỀU KHIỂN MÁY CNC Giảng Viên hướng dẫn: TS Dương Minh Đức Nhóm sinh viên thực hiện: Nhóm Thành viên: MSSV Đỗ Đức Thắng 20174202 Nguyễn Văn Hiếu 20173866 Phạm Thế Thiện 20174235 Hà Nội, 6/2021 Mục lục Mục Lục PHẦN I: ĐỀ BÀI PHẦN II: BÀI LÀM .4 Thiết kế điều khiển PID vị trí cho trục .4 Nội suy 11 2.1 Nội suy đường thẳng 11 2.1.1 Cơng thức tính tốn nội suy đường thẳng 11 2.1.2 Nội suy thô …………………………………………………………… 12 2.1.3 Nội suy tinh…………………………………………………… ………13 2.1.2 Kết mô 16 2.2 Nội suy đường tròn 18 2.2.1 Xây dựng cơng thức nội suy đường trịn .19 2.2.2 Mô 20 Vẽ Chữ C 20 3.1 Tính tốn lý thuyết 20 3.2 Mô .21 Thực tăng tốc/ giảm tốc sau nội suy………………………………… 23 4.1 Tổng quan chung 24 4.2 Nội suy đoạn thẳng…………………………………………………….25 4.3 Nội suy đường tròn ………………………………………………… 27 CODE MATLAB………………………………………………………… 33 BAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.co Đề PHẦN I: ĐỀ BÀI 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 nhất: Trục x: Vx k1 = Ux T s+1 Trục y: Vy k2 = Uy T s+ Với k1≠ k2, T1≠ T2 tự chọn, T1, T2 nhỏ (< 0.5) 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 - Độ 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  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 Nội suy đường tròn tâm I(x0,y0) bán kính R tùy chọn chu kỳ nội suy 5ms, chu kì điều khiển 1ms Yêu cầu gia tốc giảm tốc trước nội suy Xác định quỹ đạo chuyển động theo thời gian với profile vận tốc hình thang, amax vmax tự chọn Chia tiếp 5ms nội suy theo phương pháp tuyến tính trung bình BAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.co BAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.co Đề 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) Gợi ý: Từ quỹ đạo tính bước di chuyển cho trục, sau dùng lọc số để thực tăng tốc/giảm tốc Ghi chú: Mỗi nhóm sinh viên chuẩn bị báo cáo, có kèm mã chương trình mơ File code chương trình mô phải nộp cho giáo viên BAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.co BAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.co Nội suy đường thẳng PHẦN II: BÀI LÀM Thiết kế điều khiển PID vị trí cho trục * Hai hàm truyền đối tượng: - Trục x: G1(s)¿ k1 = T +1 0.02 s+1 - Trục y: G2(s)¿ k2 = T +1 0.01 s+1 * Mô matlab: + Mục tiêu thiết kế:  Đáp ứng nhanh tốt  |Udk| = 20 && X_CD(i) = xO2 + (X_CD(i-1) - xO2)*cos(angle(i)) + (yO2 - Y_CD(i1))*sin(angle(i)); Y_CD(i) = yO2 - (yO2 - Y_CD(i-1))*cos(angle(i)) + (X_CD(i-1) xO2)*sin(angle(i)); end %ve cung CD %noi suy tho doan thang BC c =(xC-xB)/L_BC; s =(yC-yB)/L_BC; Tc = (L_BC - F^2/(2*A)-F^2/(2*D))/F; T_BC = Ta + Td + Tc; %ma tran bo nho nA = 0:1:(Ta/Tipo); %them gia tri the hien toc ban dau la nT = 1:1:(T_BC/Tipo); nC = 1:1:(Tc/Tipo); nD = 1:1:(Td/Tipo); %tinh toc Va = + A*nA*Tipo; Vc = F + 0*nC*Tipo; Vd = F - D*nD*Tipo; V = [Va, Vc, Vd]; dL_BC = V; for i=1:(length(V)-1) dL_BC(i) = ((V(i)+V(i+1))/2)*Tipo; end X_BC = xB + cumsum(dL_BC)*c; Y_BC = yB + cumsum(dL_BC)*s; % ve noi suy figure(2) stairs(X_AB, hold on stairs(X_BC, hold on stairs(X_CD, grid on hold on tho chu C Y_AB, 'linewidth', 1.5) Y_BC, 'linewidth', 1.5) Y_CD, 'linewidth', 1.5) BAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.co 36 BAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.co Tăng giảm tóc sau nội suy axis([-10 100, -10 100]) axis equal xlabel('X') ylabel('Y') title('Noi suy tho chu C') %noi suy tinh cung AB N = Tipo/Tdk; angle_t = angle/N; t=1; X_t_AB(t) = xA; Y_t_AB(t) = yA; for i = 1:length(angle_t) if i == for j = 2:N t = t+1; X_t_AB(t) = xO1 + Y_t_AB(t-1))*sin(angle_t(i)); Y_t_AB(t) = yO1 1) - xO1)*sin(angle_t(i)); end else for j = 1:N t = t+1; X_t_AB(t) = xO1 + Y_t_AB(t-1))*sin(angle_t(i)); Y_t_AB(t) = yO1 1) - xO1)*sin(angle_t(i)); end end end %noi suy tinh cung CD angle_t = angle/N; t=1; X_t_CD(t) = xC; Y_t_CD(t) = yC; for i = 1:length(angle_t) if i == for j = 2:N t = t+1; X_t_CD(t) = xO2 + Y_t_CD(t-1))*sin(angle_t(i)); Y_t_CD(t) = yO2 1) - xO2)*sin(angle_t(i)); end else for j = 1:N t = t+1; X_t_CD(t) = xO2 + Y_t_CD(t-1))*sin(angle_t(i)); Y_t_CD(t) = yO2 1) - xO2)*sin(angle_t(i)); end end end (X_t_AB(t-1) - xO1)*cos(angle_t(i)) + (yO1 (yO1 - Y_t_AB(t-1))*cos(angle_t(i)) + (X_t_AB(t- (X_t_AB(t-1) - xO1)*cos(angle_t(i)) + (yO1 (yO1 - Y_t_AB(t-1))*cos(angle_t(i)) + (X_t_AB(t- (X_t_CD(t-1) - xO2)*cos(angle_t(i)) + (yO2 (yO2 - Y_t_CD(t-1))*cos(angle_t(i)) + (X_t_CD(t- (X_t_CD(t-1) - xO2)*cos(angle_t(i)) + (yO2 (yO2 - Y_t_CD(t-1))*cos(angle_t(i)) + (X_t_CD(t- %noi suy tinh doan BC dL_t_BC = dL_BC(1)/N; for i = 1:length(dL_BC) if i == for j = 1:N-1 dL_t_BC = [dL_t_BC, dL_BC(i)/N]; BAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.co 37 BAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.co Tăng giảm tóc sau nội suy end else for j = 1:N dL_t_BC = [dL_t_BC, dL_BC(i)/N]; end end end X_t_BC = xB + cumsum(dL_t_BC)*c; Y_t_BC = yB + cumsum(dL_t_BC)*s; %ve noi suy tinh va tho hinh chu C figure(2) stairs(X_t_AB, Y_t_AB, 'linewidth', 1.5) hold on stairs(X_t_BC, Y_t_BC, 'linewidth', 1.5) hold on stairs(X_t_CD, Y_t_CD, 'linewidth', 1.5) grid on hold on axis([-10 100, -10 100]) axis equal xlabel('X') ylabel('Y') title('Noi suy tho va tinh chu C') %ve van toc an dao figure(3) time_AB = Tipo*(1:(fix(T_AB/Tipo)+2)); time_BC = max(time_AB) + (Tipo*(1:(fix(T_BC/Tipo)+1))); time_CD = max(time_BC) + (Tipo*(1:(fix(T_CD/Tipo)+2))); plot(time_AB', angle_AB*R/Tipo, 'linewidth', 1.5) hold on plot(time_CD', angle_CD*R/Tipo, 'linewidth', 1.5) hold on plot(time_BC', dL_BC/Tipo, 'linewidth', 1.5) hold on grid on axis([-1 12, -1 30]) xlabel('t(s)') ylabel('V(mm/s)') title('do thi van toc') % Import Data to Simulink T = T_AB + T_BC + T_CD; X = [X_t_AB, X_t_BC, X_t_CD]; Y = [Y_t_AB, Y_t_BC, Y_t_CD]; time = Tipo*(1:(fix(T/Tipo)+5)*5); % Chu ki trich mau 1ms in_x = [time', X']; in_y = [time', Y']; %ve tin hieu suat tu simulink %x=out_x.Data; %y=out_y.Data; %plot(x,y,'linewidth',1.5) %xlabel('X') %ylabel('Y') %title('simulink chu C') %dau X figure(1) plot(time', X', 'linewidth', 1.5) grid on axis([-1 65, -1 85]) xlabel('t(s)') ylabel('X') title('dau X') %dau Y BAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.co 38 BAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.co Tăng giảm tóc sau nội suy figure(4) plot(time', Y', 'linewidth', 1.5) grid on axis([-1 65, -1 85]) xlabel('t(s)') ylabel('Y') title('dau Y') Tăng/Giảm tốc sau nội suy đường thẳng %tang giam toc sau noi suy doan thang clear clc A = 20; D = 20; F = 20; Tipo = 5e-3; Tdk = 1e-3; xA = 30; yA = 50; xB = 90; yB = 80; L = sqrt((xB-xA)^2+(yB-yA)^2); cos =(xB-xA)/L; sin =(yB-yA)/L; Ta = F/A; T = L/F; nT = 1:1:(ceil(T/Tipo)); k = zeros(1, ceil(Ta/Tipo)); n = length(k); dL = L/max(nT); for i=2:length(nT) dL = [dL, L/max(nT)]; end %ap dung cong thuc S(k) = S(k ?1) + ?X(k)??X(k ?n) % ?Xo(k) = S(k)/n P = [dL, k]; %dong vai tro la ?X(k) Q = [k,dL]; %dong vai tro la ?X(k ?n) S = P/n; %dong vai tro la ?Xo(k) for i=2:length(P) S(i) = S(i-1) + (P(i) - Q(i))/n; end X = xA + cumsum(S)*cos; Y = yA + cumsum(S)*sin; %do thi noi suy tho doan thang figure(1) stairs(X, Y, 'linewidth', 1.5) grid on hold on axis([-10 100, -10 100]) axis equal xlabel('X') ylabel('Y') title('Noi suy tho doan thang AB') % Import Data to Simulink time = Tipo*(1:(ceil(T/Tipo)+n)); % Chu ki trich mau 1ms in_x = [time', X']; in_y = [time', Y']; BAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.co 39 BAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.co Tăng giảm tóc sau nội suy %do thi bieu thi toc figure(2) plot(time', S/Tipo, 'linewidth', 1.5) grid on hold on axis([-1 5, -1 30]) xlabel('X') ylabel('Y') title('do thi bieu thi toc do') %dau X figure(3) plot(time', X', 'linewidth', 1.5) grid on xlabel('t(s)') ylabel('X') title('dau X') %dau Y figure(4) plot(time', Y', 'linewidth', 1.5) grid on xlabel('t(s)') ylabel('Y') title('dau Y') Tăng/Giảm tốc sau nội suy đường tròn: %tang giam toc sau noi suy duong tron clear clc A = 20; D = 20; F = 20; Tipo = 5e-3; Tdk = 1e-3; %thong so duong tron xA = 20; yA = 00; %diem bat dau xB = 40; yB = 20; %diem ket thuc xO = 20; yO = 20; %tam duong tron R = 20; vecOA = [xA-xO yA-yO]; vecOB = [xB-xO yB-yO]; L_OA = sqrt(vecOA(1)^2 + vecOA(2)^2); L_OB = sqrt(vecOB(1)^2 + vecOB(2)^2); angleAOB = acos(dot(vecOA, vecOB)/(L_OA*L_OB)); %L = R * angleAOB; %do dai cung tron L = R*2*pi; Ta = F/A; T = L/F; nT = 1:1:(ceil(T/Tipo)); k = zeros(1, ceil(Ta/Tipo)); n = length(k); %tinh to goc W = F/R; angle = W*Tipo; for i=1:length(nT) angle = [angle, W*Tipo]; BAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.co 40 BAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.co Tăng giảm tóc sau nội suy end %ap % P = Q = S = for dung cong thuc S(k) = S(k ?1) + ?X(k)??X(k ?n) ?Xo(k) = S(k)/n [angle, k]; %dong vai tro la ?X(k) [k, angle]; %dong vai tro la ?X(k ?n) P/n; %dong vai tro la ?Xo(k) i=2:length(P) S(i) = S(i-1) + (P(i) - Q(i))/n; end X = S; Y = S; X(1) = xA; Y(1) = yA; for i = 2:length(S) X(i) = xO + (X(i-1) - xO)*cos(S(i)) + (yO - Y(i-1))*sin(S(i)); Y(i) = yO - (yO - Y(i-1))*cos(S(i)) + (X(i-1) - xO)*sin(S(i)); end figure(1) stairs(X, Y, 'linewidth', 1.5) grid on hold on axis([-10 50, -10 50]) axis equal xlabel('X') ylabel('Y') title('Noi suy tho duong tron') %Import Data to Simulink time = Tipo*(1:(ceil(T/Tipo)+n+1)); % Chu ki trich mau 1ms in_x = [time', X']; in_y = [time', Y']; %dau X figure(2) plot(time', X', 'linewidth', 1.5) grid on xlabel('t(s)') ylabel('X') title('dau X') %dau Y figure(3) plot(time', Y', 'linewidth', 1.5) grid on xlabel('t(s)') ylabel('Y') title('dau Y') %do thi bieu thi toc dai figure(4) plot(time', (S/Tipo)*R, 'linewidth', 1.5) grid on hold on axis([-1 10, -1 30]) xlabel('time') ylabel('V') title('do thi bieu thi toc dai') BAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.co 41 BAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.co BAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.coBAO.cao.bai.tap.lon.he.THONG.dieu.KHIEN.may.CNC.mot.he.thong.CNC.gom.2.truc.x.va.y.duoc.dieu.khien.boi.2.dong.co