Đang tải... (xem toàn văn)
CHƯƠNG 1. Giới thiệu tổng quan về robot YASKAWA EP4000D11.1Giới thiệu về hãng YASKAWA11.2.Giới thiệu về Robot EP4000D1CHƯƠNG 2. Động học thuận vị trí52.1 . Xây dựng chương trình động học thuận52.11 .Gắn trục tọa độ lên các khớp52.1.2. Xác định các ma trận A62.1.3. Xác định các ma trận T72.2. Xây dựng phần mềm tính toán động lực học thuận11CHƯƠNG 3. Ma trận Jacoby163.1.Xây dựng ma trận Jacoby163.2.Thực hiện trên MatLab163.3.Xây dụng giao diện phần mềm tính toán Jacoby18CHƯƠNG 4. Động học đảo vị trí214.1. Lập phương trình tính toán động học đảo vị trí robot214.1.1 Tính ma trận nghịch đảo của các ma trận vị trí khớp214.1.2. Tính toán động học ngược robot theo phương pháp phân ly biến224.2. Xây dựng giao diện nhập xuất dữ liệu và hiển thị kết quả24CHƯƠNG 5. Thiết kế quỹ đạo chuyển động của các khớp Robot theo quỹ đạo dạng bậc 3275.1. Quỹ đạo đa thức bậc 3 đi qua hai điểm đơn275.2. Quỹ đạo đa thức bậc 3 đi qua 1 điểm trung gian θv28CHƯƠNG 6. Xây dựng mô hình động lực học cho đối tượng trênToolBox SimmechanicsMatLab 306.1. Mô phỏng Robot trên MatLab306.2. Cấu trúc sơ đồ điều khiển các trục của robot306.3. Giao diện bảng điều khiển316.4. Chương trình điều khiển32
MỤC LỤC Chương 1: Giới thiệu tổng quan robot YASKAWA EP4000D Giới thiệu hãng hãng YASKAWA 1.1 Robot EP4000D hãng YASKAWA Nhật Bản thiết kế chế tạo Hãng thành lập năm 1915 với lịch sử 100 năm, tập đoàn hàng đầu giới sản xuất cung cấp sản phẩm lĩnh vực robot công nghiệp, biến tần, truyền động điện Các sản phẩm Yaskawa kết hợp thành tựu nghiên cứu phát triển điện tử kết tinh sản phẩm Yaskawa công ty giới đưa khái niệm “Mechatronics – điện tử” tiếng trở thành lĩnh vực mũi nhọn thu hút nhiều quan tâm tạo sản phẩm tự động hóa đáp ứng yêu cầu ngày cao đời sống sản xuất Giới thiệu Robot EP4000D 1.2 Robot EP4000D phát triển để ứng dụng vào dây truyền sản xuất công nghiệp với phù hợp với nhiều công việc khác như: • • • • • Robot gắp (Handling) Robot nâng bốc, đóng gói ( Picking/packing, palletizing) Robot hàn, hàn điểm ( Arc handling, spot welding) Robot sơn ( Painting) Robot lắp ráp ( Assembly/distributing) Robot YASKAWA EP4000D Page Do việc sử dụng sản phẩm robot EP4000D Yaskawa đảm bảo tính tương thích, độ quán chất lượng dịch vụ cao việc tự động hóa máy móc, dây chuyền sản xuất Yaskawa cung cấp tát loại robot để giải toán đáp ứng yêu cầu khác khách hàng Các robot EP4000D có trọng tải 200 kg, chiều ngang ngang 3,505 mm tầm với thẳng đứng 2,629 mm Thiết kế gọn nhẹ robot cho phép làm việc không gian chật hẹp cách kinh hoạt Đây loại robot có sáu trục quay vận hành khí nén kết hợp với động điện, giúp robot vận hành cách linh hoạt, di truyển dễ dàng dến vị trí đảm bảo độ xác robot cơng việc Hình ảnh bên cho ta thấy kích thước robot, vùng làm việc robot vùng có màu xanh Robot YASKAWA EP4000D Page Thơng số kĩ thuật robot • Bộ điều khiển Robot Robot YASKAWA EP4000D Page Robot dùng diều khiển DX100, giúp người vận hành dễ dàng lập trình điều khiển robot, dễ dàng kết nối robot với hệ thống dây chuyền sản xuất Dưới thông số kĩ thuật điều khiển Chương 2: Động học thuận vị trí Robot YASKAWA EP4000D Page 2.1 Xây dựng phương trình động học thuận 2.1.1 Gắn trục tọa độ lên khâu Hình 2.1 Lựa chọn hệ tọa độ cho từn khớp robot Bảng 2.1 Bảng thông số Denavit-Hartenberg i 550 1100 250 0 Robot YASKAWA EP4000D αi 90 90 -90 90 Page θi θ1* θ2* θ3* θ4* θ5* θ6* di 740 2135 255 2.1.2 Xác định ma trận A Ma trận An có dạng : Thực matlab, ta đặt θ = i >> syms a1 d1 a2 a3 d3 d5; >> syms i1 i2 i3 i4 i5 i6; >> c1=cos(i1);c2=cos(i2);c3=cos(i3);c4=cos(i4); c5=cos(i5);c6=cos(i6); >> s1=sin(i1);s2=sin(i2);s3=sin(i3);s4=sin(i4); s5=sin(i5);s6=sin(i6); >> A1 = [c1 s1 a1*c1; s1 -c1 a1*s1; d1 ;0 0 1]; >> A2=[c2 -s2 a2*s2; s2 c2 a2*s2; 0 ;0 0 1]; >> A3=[c3 s3 a3*c3; s3 -c3 a3*s3; d3; 0 1]; >> A4=[c4 -s4 0; s4 c4 0; -1 0; 0 1]; >> A5=[c5 s5 0; s5 -c5 0; d5; 0 1]; >>A6=[c6 -s6 0; s6 c6 0; 0 0;0 0 1]; Bảng 2.2 Ma trận định vị theo khớp Robot YASKAWA EP4000D Page A1= [cos (i1), [sin(i1), [0 ,1 [0 , , sin(i1), a1*cos(i1)] , -cos(i1), a1*sin(i1)] ,0 , d1 ] , , ] A2= [cos(i2), -sin(i2), 0, a2*sin(i2)] [sin(i2), cos(i2), 0, a2*sin(i2)] [ 0, 0, 1, ] [ 0, 0, 0, ] A3= [cos(i3), 0, sin(i3), a3*cos(i3) ] [sin(i3), 0, -cos(i3), a3*sin(i3)] [ 0, 1, 0, d3 ] [ 0, 0, 0, ] A4= [cos(i4), 0, -sin(i4), ] [sin(i4), 0, cos(i4), a4*sin(i4) ] [ 0, -1, 0, ] [ 0, 0, 0, ] A5= [cos(i5), 0, sin(i5), ] [sin(i5), 0, -cos(i5), ] [ 0, 1, 0, d5 ] [ 0, 0, 0, ] A6= [cos(i6), -sin(i6), 0, [sin(i6), cos(i6), 0, [ 0, 0, 1, [ 0, 0, 0, ] 0] 0] 1] 2.1.3 Xác định ma trận T theo hệ tọa độ từ khâu cuối * = >> T56 = A6; T56 = [ cos(i6), -sin(i6), 0, 0] [ sin(i6), cos(i6), 0, 0] [ 0, 0, 1, 0] [ 0, 0, 0, 1] *= Robot YASKAWA EP4000D Page >> T46=A5*A6 T46 = [ cos(i5)*cos(i6), -cos(i5)*sin(i6), sin(i5), 0] [ cos(i6)*sin(i5), -sin(i5)*sin(i6), -cos(i5), 0] [ sin(i6), [ cos(i6), 0, 0, 0, d5] 0, 1] * = T46 T36 = [ cos(i4)*cos(i5)*cos(i6) sin(i4)*sin(i6), cos(i4)*cos(i5)*sin(i6), cos(i4)*sin(i5), -d5*sin(i4)] - [ cos(i4)*sin(i6) + cos(i5)*cos(i6)*sin(i4), -cos(i5)*sin(i4)*sin(i6), sin(i4)*sin(i5), d5*cos(i4)] [ -cos(i6)*sin(i5), sin(i5)*sin(i6), [ 0, 0, 0, cos(i5), cos(i6)*sin(i4) - cos(i4)*cos(i6) 0] 1] * = T36 T26 = [ - cos(i3)*(sin(i4)*sin(i6) - cos(i4)*cos(i5)*cos(i6)) - cos(i6)*sin(i3)*sin(i5), sin(i3)*sin(i5)*sin(i6) - cos(i3)*(cos(i6)*sin(i4) + cos(i4)*cos(i5)*sin(i6)), cos(i5)*sin(i3) + cos(i3)*cos(i4)*sin(i5), a3*cos(i3) - d5*cos(i3)*sin(i4)] [ cos(i3)*cos(i6)*sin(i5) - sin(i3)*(sin(i4)*sin(i6) - cos(i4)*cos(i5)*cos(i6)), sin(i3)*(cos(i6)*sin(i4) + cos(i4)*cos(i5)*sin(i6)) - cos(i3)*sin(i5)*sin(i6), cos(i4)*sin(i3)*sin(i5) - cos(i3)*cos(i5), a3*sin(i3) - d5*sin(i3)*sin(i4)] [cos(i4)*sin(i6) + cos(i5)*cos(i6)*sin(i4), cos(i4)*cos(i6) - cos(i5)*sin(i4)*sin(i6), sin(i4)*sin(i5), d3 + d5*cos(i4)] [ 0, 0, 0, 1] * = T26 Robot YASKAWA EP4000D Page T16 = [ sin(i2)*(sin(i3)*(sin(i4)*sin(i6) - cos(i4)*cos(i5)*cos(i6)) cos(i3)*cos(i6)*sin(i5)) - cos(i2)*(cos(i3)*(sin(i4)*sin(i6) cos(i4)*cos(i5)*cos(i6)) + cos(i6)*sin(i3)*sin(i5)), sin(i2)*(sin(i3)*(cos(i6)*sin(i4) + cos(i4)*cos(i5)*sin(i6)) + cos(i3)*sin(i5)*sin(i6)) - cos(i2)*(cos(i3)*(cos(i6)*sin(i4) + cos(i4)*cos(i5)*sin(i6)) - sin(i3)*sin(i5)*sin(i6)), cos(i2)*(cos(i5)*sin(i3) + cos(i3)*cos(i4)*sin(i5)) + sin(i2)*(cos(i3)*cos(i5) - cos(i4)*sin(i3)*sin(i5)), cos(i2)*(a3*cos(i3) - d5*cos(i3)*sin(i4)) + a2*sin(i2) - sin(i2)*(a3*sin(i3) d5*sin(i3)*sin(i4))] [ - cos(i2)*(sin(i3)*(sin(i4)*sin(i6) - cos(i4)*cos(i5)*cos(i6)) cos(i3)*cos(i6)*sin(i5)) - sin(i2)*(cos(i3)*(sin(i4)*sin(i6) cos(i4)*cos(i5)*cos(i6)) + cos(i6)*sin(i3)*sin(i5)), cos(i2)*(sin(i3)*(cos(i6)*sin(i4) + cos(i4)*cos(i5)*sin(i6)) + cos(i3)*sin(i5)*sin(i6)) - sin(i2)*(cos(i3)*(cos(i6)*sin(i4) + cos(i4)*cos(i5)*sin(i6)) - sin(i3)*sin(i5)*sin(i6)), sin(i2)*(cos(i5)*sin(i3) + cos(i3)*cos(i4)*sin(i5)) - cos(i2)*(cos(i3)*cos(i5) - cos(i4)*sin(i3)*sin(i5)), sin(i2)*(a3*cos(i3) - d5*cos(i3)*sin(i4)) + a2*sin(i2) + cos(i2)*(a3*sin(i3) d5*sin(i3)*sin(i4))] [ cos(i4)*sin(i6) + cos(i5)*cos(i6)*sin(i4), cos(i4)*cos(i6) - cos(i5)*sin(i4)*sin(i6), sin(i4)*sin(i5), d3 + d5*cos(i4)] [ 0, 0, 0, ] * = T16 T06 = [sin(i1)*(cos(i4)*sin(i6)+cos(i5)*cos(i6)*sin(i4)) cos(i1)*(cos(i2)*(cos(i3)*(sin(i4)*sin(i6) cos(i4)*cos(i5)*cos(i6)) + cos(i6)*sin(i3)*sin(i5)) - sin(i2)*(sin(i3)*(sin(i4)*sin(i6) - cos(i4)*cos(i5)*cos(i6)) Robot YASKAWA EP4000D Page 10 - Giả sử đường cong xuất phát từ t = dừng t = ti (i = 1,2), chọn t1 = t2 Các ràng buộc đưa ra: + Cân phương trình vị trí điểm đầu/cuối trung gian: + cân phương trình tốc độ điểm đầu cuối: + phương trình cân tốc độ điểm trung gian: + phương trình cân gia tốc điểm trung gian: = - thay vào ta xác định ai: Chương Xây dựng mô hình động lực học cho đối tượng ToolBox Simmechanics/MatLab 6.1 Mô Robot MatLab Robot YASKAWA EP4000D Page 30 6.2 Cấu trúc sơ đồ điều khiển trục robot Bộ điều khiển góc quay robot Robot YASKAWA EP4000D Page 31 6.3 Giao diện bảng điều khiển Bảng điều khiển có Silder để giúp chọn góc cho trục Robot Và kết Px, Py, Pz lên hình 6.4 Chương trình điều khiển function varargout = bang_dieu_khien(varargin) % Begin initialization code - DO NOT EDIT gui_Singleton = 1; gui_State = struct('gui_Name', mfilename, 'gui_Singleton', gui_Singleton, 'gui_OpeningFcn', @bang_dieu_khien_OpeningFcn, Robot YASKAWA EP4000D Page 32 'gui_OutputFcn', @bang_dieu_khien_OutputFcn, 'gui_LayoutFcn', [] , 'gui_Callback', []); if nargin && ischar(varargin{1}) gui_State.gui_Callback = str2func(varargin{1}); end if nargout [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:}); else gui_mainfcn(gui_State, varargin{:}); end handles.output = hObject; varargout{1} = handles.output; function slider1_Callback(hObject, eventdata, handles) % Khai bao a,d a1=550; d1=740; a2=1100; a3=250; d3=2135; d5=255; ModelName = 'robot2'; global var; t1=get(handles.slider1, 'value'); %lay cac gia tri tu slider set(handles.edit1,'string',num2str(t1)); %hien thi tri bang duoi t2=get(handles.slider2, 'value'); set(handles.edit2,'string',num2str(t2)); t3=get(handles.slider3, 'value'); set(handles.edit3,'string',num2str(t3)); t4=get(handles.slider4, 'value'); set(handles.edit4,'string',num2str(t4)); t5=get(handles.slider5, 'value'); set(handles.edit5,'string',num2str(t5)); t6=get(handles.slider6, 'value'); set(handles.edit6,'string',num2str(t6)); set_param([ModelName '/Slider Gain'], 'Gain',num2str(t1)) set_param([ModelName '/Slider Gain1'], 'Gain',num2str(t2)) set_param([ModelName '/Slider Gain2'], 'Gain',num2str(t3)) set_param([ModelName '/Slider Gain3'], 'Gain',num2str(t4)) set_param([ModelName '/Slider Gain4'], 'Gain',num2str(t5)) set_param([ModelName '/Slider Gain5'], 'Gain',num2str(t6)) c1=cos(t1);c2=cos(t2);c3=cos(t3);c4=cos(t4); c5=cos(t5);c6=cos(t6); s1=sin(t1);s2=sin(t2);s3=sin(t3);s4=sin(t4); s5=sin(t5);s6=sin(t6); T1 = [c1 s1 a1*c1; s1 -c1 a1*s1; d1 ;0 0 1]; Robot YASKAWA EP4000D Page 33 T2=[c2 -s2 a2*s2; s2 c2 a2*s2; 0 ;0 0 1]; T3=[c3 s3 a3*c3; s3 -c3 a3*s3; d3; 0 1]; T4=[c4 -s4 0; s4 c4 0; -1 0; 0 1]; T5=[c5 s5 0; s5 -c5 0; d5; 0 1]; T6=[c6 -s6 0; s6 c6 0; 0 0;0 0 1]; T=T1*T2*T3*T4*T5*T6; Px=T(1,4); Py=T(2,4); Pz=T(3,4); set(handles.edit7, 'string', num2str(Px)); set(handles.edit8, 'string', num2str(Py)); set(handles.edit9, 'string', num2str(Pz)); % - Executes during object creation, after setting all properties function slider1_CreateFcn(hObject, eventdata, handles) if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor',[.9 9]); end % - Executes on slider movement function slider2_Callback(hObject, eventdata, handles) a1=550; d1=740; a2=1100; a3=250; d3=2135; d5=255; ModelName = 'robot2'; global var; t1=get(handles.slider1, 'value'); %lay cac gia tri tu slider set(handles.edit1,'string',num2str(t1)); %hien thi tri bang duoi t2=get(handles.slider2, 'value'); set(handles.edit2,'string',num2str(t2)); t3=get(handles.slider3, 'value'); set(handles.edit3,'string',num2str(t3)); t4=get(handles.slider4, 'value'); set(handles.edit4,'string',num2str(t4)); t5=get(handles.slider5, 'value'); set(handles.edit5,'string',num2str(t5)); t6=get(handles.slider6, 'value'); set(handles.edit6,'string',num2str(t6)); set_param([ModelName '/Slider Gain'], 'Gain',num2str(t1)) set_param([ModelName '/Slider Gain1'], 'Gain',num2str(t2)) set_param([ModelName '/Slider Gain2'], 'Gain',num2str(t3)) set_param([ModelName '/Slider Gain3'], 'Gain',num2str(t4)) Robot YASKAWA EP4000D Page 34 set_param([ModelName '/Slider Gain4'], 'Gain',num2str(t5)) set_param([ModelName '/Slider Gain5'], 'Gain',num2str(t6)) c1=cos(t1);c2=cos(t2);c3=cos(t3);c4=cos(t4); c5=cos(t5);c6=cos(t6); s1=sin(t1);s2=sin(t2);s3=sin(t3);s4=sin(t4); s5=sin(t5);s6=sin(t6); T1 = [c1 s1 a1*c1; s1 -c1 a1*s1; d1 ;0 0 1]; T2=[c2 -s2 a2*s2; s2 c2 a2*s2; 0 ;0 0 1]; T3=[c3 s3 a3*c3; s3 -c3 a3*s3; d3; 0 1]; T4=[c4 -s4 0; s4 c4 0; -1 0; 0 1]; T5=[c5 s5 0; s5 -c5 0; d5; 0 1]; T6=[c6 -s6 0; s6 c6 0; 0 0;0 0 1]; T=T1*T2*T3*T4*T5*T6; Px=T(1,4); Py=T(2,4); Pz=T(3,4); set(handles.edit7, 'string', num2str(Px)); set(handles.edit8, 'string', num2str(Py)); set(handles.edit9, 'string', num2str(Pz)); function slider2_CreateFcn(hObject, eventdata, handles) if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor',[.9 9]); end % - Executes on slider movement function slider3_Callback(hObject, eventdata, handles) % Khai bao a,d a1=550; d1=740; a2=1100; a3=250; d3=2135; d5=255; ModelName = 'robot2'; global var; t1=get(handles.slider1, 'value'); %lay cac gia tri tu slider set(handles.edit1,'string',num2str(t1)); %hien thi tri bang duoi t2=get(handles.slider2, 'value'); set(handles.edit2,'string',num2str(t2)); t3=get(handles.slider3, 'value'); set(handles.edit3,'string',num2str(t3)); t4=get(handles.slider4, 'value'); set(handles.edit4,'string',num2str(t4)); t5=get(handles.slider5, 'value'); set(handles.edit5,'string',num2str(t5)); t6=get(handles.slider6, 'value'); set(handles.edit6,'string',num2str(t6)); set_param([ModelName '/Slider Gain'], 'Gain',num2str(t1)) set_param([ModelName '/Slider Gain1'], 'Gain',num2str(t2)) set_param([ModelName '/Slider Gain2'], 'Gain',num2str(t3)) Robot YASKAWA EP4000D Page 35 set_param([ModelName '/Slider Gain3'], 'Gain',num2str(t4)) set_param([ModelName '/Slider Gain4'], 'Gain',num2str(t5)) set_param([ModelName '/Slider Gain5'], 'Gain',num2str(t6)) c1=cos(t1);c2=cos(t2);c3=cos(t3);c4=cos(t4); c5=cos(t5);c6=cos(t6); s1=sin(t1);s2=sin(t2);s3=sin(t3);s4=sin(t4); s5=sin(t5);s6=sin(t6); T1 = [c1 s1 a1*c1; s1 -c1 a1*s1; d1 ;0 0 1]; T2=[c2 -s2 a2*s2; s2 c2 a2*s2; 0 ;0 0 1]; T3=[c3 s3 a3*c3; s3 -c3 a3*s3; d3; 0 1]; T4=[c4 -s4 0; s4 c4 0; -1 0; 0 1]; T5=[c5 s5 0; s5 -c5 0; d5; 0 1]; T6=[c6 -s6 0; s6 c6 0; 0 0;0 0 1]; T=T1*T2*T3*T4*T5*T6; Px=T(1,4); Py=T(2,4); Pz=T(3,4); set(handles.edit7, 'string', num2str(Px)); set(handles.edit8, 'string', num2str(Py)); set(handles.edit9, 'string', num2str(Pz)); function slider3_CreateFcn(hObject, eventdata, handles) if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor',[.9 9]); end % - Executes on slider movement function slider4_Callback(hObject, eventdata, handles) % Khai bao a,d a1=550; d1=740; a2=1100; a3=250; d3=2135; d5=255; ModelName = 'robot2'; global var; t1=get(handles.slider1, 'value'); %lay cac gia tri tu slider set(handles.edit1,'string',num2str(t1)); %hien thi tri bang duoi t2=get(handles.slider2, 'value'); set(handles.edit2,'string',num2str(t2)); t3=get(handles.slider3, 'value'); set(handles.edit3,'string',num2str(t3)); t4=get(handles.slider4, 'value'); set(handles.edit4,'string',num2str(t4)); t5=get(handles.slider5, 'value'); Robot YASKAWA EP4000D Page 36 set(handles.edit5,'string',num2str(t5)); t6=get(handles.slider6, 'value'); set(handles.edit6,'string',num2str(t6)); set_param([ModelName '/Slider Gain'], 'Gain',num2str(t1)) set_param([ModelName '/Slider Gain1'], 'Gain',num2str(t2)) set_param([ModelName '/Slider Gain2'], 'Gain',num2str(t3)) set_param([ModelName '/Slider Gain3'], 'Gain',num2str(t4)) set_param([ModelName '/Slider Gain4'], 'Gain',num2str(t5)) set_param([ModelName '/Slider Gain5'], 'Gain',num2str(t6)) c1=cos(t1);c2=cos(t2);c3=cos(t3);c4=cos(t4); c5=cos(t5);c6=cos(t6); s1=sin(t1);s2=sin(t2);s3=sin(t3);s4=sin(t4); s5=sin(t5);s6=sin(t6); T1 = [c1 s1 a1*c1; s1 -c1 a1*s1; d1 ;0 0 1]; T2=[c2 -s2 a2*s2; s2 c2 a2*s2; 0 ;0 0 1]; T3=[c3 s3 a3*c3; s3 -c3 a3*s3; d3; 0 1]; T4=[c4 -s4 0; s4 c4 0; -1 0; 0 1]; T5=[c5 s5 0; s5 -c5 0; d5; 0 1]; T6=[c6 -s6 0; s6 c6 0; 0 0;0 0 1]; T=T1*T2*T3*T4*T5*T6; Px=T(1,4); Py=T(2,4); Pz=T(3,4); set(handles.edit7, 'string', num2str(Px)); set(handles.edit8, 'string', num2str(Py)); set(handles.edit9, 'string', num2str(Pz)); function slider4_CreateFcn(hObject, eventdata, handles) if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor',[.9 9]); end % - Executes on slider movement function slider5_Callback(hObject, eventdata, handles) % Khai bao a,d a1=550; d1=740; a2=1100; a3=250; d3=2135; d5=255; ModelName = 'robot2'; global var; t1=get(handles.slider1, 'value'); %lay cac gia tri tu slider set(handles.edit1,'string',num2str(t1)); %hien thi tri bang duoi t2=get(handles.slider2, 'value'); set(handles.edit2,'string',num2str(t2)); Robot YASKAWA EP4000D Page 37 t3=get(handles.slider3, 'value'); set(handles.edit3,'string',num2str(t3)); t4=get(handles.slider4, 'value'); set(handles.edit4,'string',num2str(t4)); t5=get(handles.slider5, 'value'); set(handles.edit5,'string',num2str(t5)); t6=get(handles.slider6, 'value'); set(handles.edit6,'string',num2str(t6)); set_param([ModelName '/Slider Gain'], 'Gain',num2str(t1)) set_param([ModelName '/Slider Gain1'], 'Gain',num2str(t2)) set_param([ModelName '/Slider Gain2'], 'Gain',num2str(t3)) set_param([ModelName '/Slider Gain3'], 'Gain',num2str(t4)) set_param([ModelName '/Slider Gain4'], 'Gain',num2str(t5)) set_param([ModelName '/Slider Gain5'], 'Gain',num2str(t6)) c1=cos(t1);c2=cos(t2);c3=cos(t3);c4=cos(t4); c5=cos(t5);c6=cos(t6); s1=sin(t1);s2=sin(t2);s3=sin(t3);s4=sin(t4); s5=sin(t5);s6=sin(t6); T1 = [c1 s1 a1*c1; s1 -c1 a1*s1; d1 ;0 0 1]; T2=[c2 -s2 a2*s2; s2 c2 a2*s2; 0 ;0 0 1]; T3=[c3 s3 a3*c3; s3 -c3 a3*s3; d3; 0 1]; T4=[c4 -s4 0; s4 c4 0; -1 0; 0 1]; T5=[c5 s5 0; s5 -c5 0; d5; 0 1]; T6=[c6 -s6 0; s6 c6 0; 0 0;0 0 1]; T=T1*T2*T3*T4*T5*T6; Px=T(1,4); Py=T(2,4); Pz=T(3,4); set(handles.edit7, 'string', num2str(Px)); set(handles.edit8, 'string', num2str(Py)); set(handles.edit9, 'string', num2str(Pz)); function slider5_CreateFcn(hObject, eventdata, handles) if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor',[.9 9]); end % - Executes on slider movement function slider6_Callback(hObject, eventdata, handles) % Khai bao a,d a1=550; d1=740; a2=1100; a3=250; d3=2135; d5=255; Robot YASKAWA EP4000D Page 38 ModelName = 'robot2'; global var; t1=get(handles.slider1, 'value'); %lay cac gia tri tu slider set(handles.edit1,'string',num2str(t1)); %hien thi tri bang duoi t2=get(handles.slider2, 'value'); set(handles.edit2,'string',num2str(t2)); t3=get(handles.slider3, 'value'); set(handles.edit3,'string',num2str(t3)); t4=get(handles.slider4, 'value'); set(handles.edit4,'string',num2str(t4)); t5=get(handles.slider5, 'value'); set(handles.edit5,'string',num2str(t5)); t6=get(handles.slider6, 'value'); set(handles.edit6,'string',num2str(t6)); set_param([ModelName '/Slider Gain'], 'Gain',num2str(t1)) set_param([ModelName '/Slider Gain1'], 'Gain',num2str(t2)) set_param([ModelName '/Slider Gain2'], 'Gain',num2str(t3)) set_param([ModelName '/Slider Gain3'], 'Gain',num2str(t4)) set_param([ModelName '/Slider Gain4'], 'Gain',num2str(t5)) set_param([ModelName '/Slider Gain5'], 'Gain',num2str(t6)) c1=cos(t1);c2=cos(t2);c3=cos(t3);c4=cos(t4); c5=cos(t5);c6=cos(t6); s1=sin(t1);s2=sin(t2);s3=sin(t3);s4=sin(t4); s5=sin(t5);s6=sin(t6); T1 = [c1 s1 a1*c1; s1 -c1 a1*s1; d1 ;0 0 1]; T2=[c2 -s2 a2*s2; s2 c2 a2*s2; 0 ;0 0 1]; T3=[c3 s3 a3*c3; s3 -c3 a3*s3; d3; 0 1]; T4=[c4 -s4 0; s4 c4 0; -1 0; 0 1]; T5=[c5 s5 0; s5 -c5 0; d5; 0 1]; T6=[c6 -s6 0; s6 c6 0; 0 0;0 0 1]; T=T1*T2*T3*T4*T5*T6; Px=T(1,4); Py=T(2,4); Pz=T(3,4); set(handles.edit7, 'string', num2str(Px)); set(handles.edit8, 'string', num2str(Py)); set(handles.edit9, 'string', num2str(Pz)); % - Executes during object creation, after setting all properties function slider6_CreateFcn(hObject, eventdata, handles) if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor',[.9 9]); Robot YASKAWA EP4000D Page 39 end function edit1_Callback(hObject, eventdata, handles) function edit1_CreateFcn(hObject, eventdata, handles) % hObject handle to edit1 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: edit controls usually have a white background on Windows % See ISPC and COMPUTER if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function edit2_Callback(hObject, eventdata, handles) function edit2_CreateFcn(hObject, eventdata, handles) % hObject handle to edit2 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: edit controls usually have a white background on Windows % See ISPC and COMPUTER if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function edit3_Callback(hObject, eventdata, handles) function edit3_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function edit4_Callback(hObject, eventdata, handles) function edit4_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function edit5_Callback(hObject, eventdata, handles) function edit5_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end Robot YASKAWA EP4000D Page 40 function edit6_Callback(hObject, eventdata, handles) function edit6_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end % - Executes on slider movement function slider7_Callback(hObject, eventdata, handles) function slider7_CreateFcn(hObject, eventdata, handles) if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor',[.9 9]); end function edit7_Callback(hObject, eventdata, handles) function edit7_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end % - Executes on slider movement function slider8_Callback(hObject, eventdata, handles) function slider8_CreateFcn(hObject, eventdata, handles) % hObject handle to slider8 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: slider controls usually have a light gray background if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor',[.9 9]); end function edit9_Callback(hObject, eventdata, handles) function edit8_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end % - Executes on slider movement function slider9_Callback(hObject, eventdata, handles) % - Executes during object creation, after setting all properties function slider9_CreateFcn(hObject, eventdata, handles) % hObject handle to slider9 (see GCBO) Robot YASKAWA EP4000D Page 41 % eventdata reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: slider controls usually have a light gray background if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor',[.9 9]); end function edit10_Callback(hObject, eventdata, handles) % - Executes during object creation, after setting all properties function edit9_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end % - Executes on button press in pushbutton1 function pushbutton1_Callback(hObject, eventdata, handles) % Khai bao a,d a1=550; d1=740; a2=1100; a3=250; d3=2135; d5=255; ModelName = 'robot2'; global var; t1=0; t1=get(handles.slider1, 'value'); %lay cac gia tri tu slider set(handles.edit1,'string',num2str(t1)); %hien thi tri bang duoi t2=0; t2=get(handles.slider2, 'value'); set(handles.edit2,'string',num2str(t2)); t3=0; t3=get(handles.slider3, 'value'); set(handles.edit3,'string',num2str(t3)); t4=0; t4=get(handles.slider4, 'value'); set(handles.edit4,'string',num2str(t4)); t5=0; t5=get(handles.slider5, 'value'); set(handles.edit5,'string',num2str(t5)); t6=0; t6=get(handles.slider6, 'value'); set(handles.edit6,'string',num2str(t6)); set_param([ModelName '/Slider Gain'], 'Gain',num2str(t1)) set_param([ModelName '/Slider Gain1'], 'Gain',num2str(t2)) set_param([ModelName '/Slider Gain2'], 'Gain',num2str(t3)) set_param([ModelName '/Slider Gain3'], 'Gain',num2str(t4)) set_param([ModelName '/Slider Gain4'], 'Gain',num2str(t5)) set_param([ModelName '/Slider Gain5'], 'Gain',num2str(t6)) Robot YASKAWA EP4000D Page 42 c1=cos(t1);c2=cos(t2);c3=cos(t3);c4=cos(t4); c5=cos(t5);c6=cos(t6); s1=sin(t1);s2=sin(t2);s3=sin(t3);s4=sin(t4); s5=sin(t5);s6=sin(t6); T1 = [c1 s1 a1*c1; s1 -c1 a1*s1; d1 ;0 0 1]; T2=[c2 -s2 a2*s2; s2 c2 a2*s2; 0 ;0 0 1]; T3=[c3 s3 a3*c3; s3 -c3 a3*s3; d3; 0 1]; T4=[c4 -s4 0; s4 c4 0; -1 0; 0 1]; T5=[c5 s5 0; s5 -c5 0; d5; 0 1]; T6=[c6 -s6 0; s6 c6 0; 0 0;0 0 1]; T=T1*T2*T3*T4*T5*T6; Px=T(1,4); Py=T(2,4); Pz=T(3,4); set(handles.edit7, 'string', num2str(Px)); set(handles.edit8, 'string', num2str(Py)); set(handles.edit9, 'string', num2str(Pz)); % - Executes on button press in pushbutton2 function pushbutton2_Callback(hObject, eventdata, handles) function pushbutton3_Callback(hObject, eventdata, handles) close; function edit8_Callback(hObject, eventdata, handles) Robot YASKAWA EP4000D Page 43 TÀI LIỆU THAM KHẢO GS.TSKH Nguyễn Văn Khang, TS Chu Anh Mỳ, 2011, Cơ sở Robot công nghiệp, Nhà xuất Giáo dục Việt Nam, Hà Nội https://www.robots.com/motoman/mpl800-ii [Ngày truy cập: 14 tháng 11 năm 2017] https://www.yaskawa.eu.com/en/products/robotic/motoman %20robots/productdetail/product/mpl800ii/ [Ngày truy cập: 14 tháng 11 năm 2017] https://www.yaskawa.eu.com/ [Ngày truy cập: 14 tháng 11 năm 2017] Robot YASKAWA EP4000D Page 44