MÔ PHỎNG VÀ ĐIỀU KHIỂN CÁNH TAY ROBOT 5 BẬC TỰ DO

Bài luận văn tốt nghiệp đại học ngành CNKT điều khiển và tự động hóa Mô phỏng và điều khiển cánh tay robot 5 bậc tự do dùng arduino

BỘ LAO ĐỘNG – THƯƠNG BINH XÃ HỘI ĐẠI HỌC SƯ PHẠM KỸ THUẬT VĨNH LONG KHÓA LUẬN TỐT NGHIỆP ĐẠI HỌC CÔNG NGHỆ KỸ THUẬT ĐIỀU KHIỂN VÀ TỰ ĐỘNG HĨA MƠ PHỎNG VÀ ĐIỀU KHIỂN CÁNH TAY ROBOT BẬC CBHD: ThS MAI NHẬT THIÊN Sinh viên: Họ tên: CHÂU TRUNG TÍN MSSV: 16007030 Họ tên: NGUYỄN TẤN THÀNH MSSV: 16007016 Vĩnh Long – năm 2019 LỜI CẢM ƠN Để hồn thành khóa luận tốt nghiệp kết thúc khóa học, với tình cảm chân thành, chúng em xin bày tỏ lòng biết ơn sâu sắc tới toàn thể cán bộ, nhân viên, giảng viên trường Đại học Sư phạm Kỹ thuật Vĩnh Long nói chung quý giảng viên khoa Điện - Điện tử nói riêng tạo điều kiện cho chúng em có mơi trường học tập tốt suốt thời gian chúng em học tập, nghiên cứu trường Chúng em xin gửi lời cảm ơn tới thầy Mai Nhật Thiên giúp đỡ chúng em suốt trình nghiên cứu trực tiếp hướng dẫn để chúng em hoàn thành khóa luận tốt nghiệp Bên cạnh đó, chúng em cảm ơn hỗ trợ, động viên gia đình bạn bè động lực to lớn để chúng em cố gắng thật tốt Chúng em xin chân thành cảm ơn! NHẬN XÉT (của giảng viên hướng dẫn)  Ý thức thực hiện: ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ………………………………………………………………………………………  Nội dung thực hiện: ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ………………………………………………………………………………………  Hình thức trình bày: ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ………………………………………………………………………………………  Tổng hợp kết quả: ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… Tổ chức báo cáo trước hội đồng Tổ chức chấm thuyết minh Vĩnh Long, ngày…tháng…năm 2019 Giảng viên hướng dẫn MAI NHẬT THIÊN DANH MỤC CÁC TỪ VIẾT TẮT ST T Chữ viết tắt DOF DH parameters FK IK c s Chữ viết đủ Dịch nghĩa Degree of Freedom Denavit-Hartenberg parameters Bậc tự Thông số DH Forward Kinematic Inverse Kinematic Cosine Sine Động học thuận Động học nghịch Hàm Cosin Hàm Sin DANH MỤC CÁC BẢNG DANH MỤC HÌNH ẢNH MỤC LỤC LỜI NÓI ĐẦU Trong thời đại cơng nghiệp hóa, robot ứng dụng vào hầu hết q trình sản xuất cơng nghiệp Việc sử dụng robot giúp cho hoạt động sản xuất nhanh chóng, xác mang lại hiệu kinh tế cao Một robot phổ biến sản xuất cánh tay robot Cánh tay robot hầu hết trường hợp lập trình sử dụng để thực nhiệm vụ cụ thể, phổ biến cho sản xuất, chế tạo ứng dụng công nghiệp Nắm bắt hữu ích yêu cầu cấp thiết việc phát triển Robot, nhóm chọn đề tài “Mô điều khiển cánh tay robot bậc” để nghiên cứu phát triển khóa luận tốt nghiệp Khóa luận tốt nghiệp chúng em từ xây dựng hàm tốn, mơ sau điều khiển thực tế, bước thực qua chương: Chương 1: Tổng quan Chương 2: Xây dựng hàm tốn cánh tay robot bậc Chương 3: Mơ phần mềm EasyRob, Solidwork, Matlab/Simulink điều khiển cánh tay robot Chương 4: Lập trình Arduino điều khiển cánh tay robot Smartphone thơng qua giao tiếp Bluetooth; trình điều khiển thông qua giao tiếp USB Chương 5: Kết Chương TỔNG QUAN 1.1 Cánh tay Robot:  Giới thiệu cánh tay robot: Cánh tay Robot robot phổ biến công nghiệp sản xuất Cánh tay robot hầu hết trường hợp lập trình sử dụng để thực nhiệm vụ cụ thể, phổ biến cho sản xuất, chế tạo ứng dụng cơng nghiệp Hình 1.1 Cánh tay robot lắp ráp ô tô nhà máy Vinfast Cánh tay robot thiết bị hoạt động theo cách tương tự cánh tay người, với số khớp di chuyển dọc theo trục xoay theo số hướng định Trên thực tế, số cánh tay robot cấu tạo lập trình hoạt động với cách bắt chước chuyển động xác cánh tay người Chúng giống cánh tay chúng ta, với cổ tay, cẳng tay, khuỷu tay khớp vai Tuy nhiên, cánh tay robot bậc tự do, cấu tạo từ trục khớp nối cho phép di chuyển, cử động theo vị trí tọa độ mong muốn  Ưu điểm cánh tay Robot: • Các chuyển động xác lặp lại giống • Hoạt động xuyên suốt; môi trường độc hại, nguy hiểm • Khả tự động hóa hoạt động theo module tốt 1.2 Phần mềm hỗ trợ mơ lập trình: 1.2.1 Phần mềm MATLAB: MATLAB (matrix laboratory) phần mềm cung cấp mơi trường tính tốn số lập trình, cơng ty MathWorks thiết kế MATLAB cho phép tính tốn số với ma trận, vẽ đồ thị hàm số hay biểu đồ thông tin, thực thuật toán, tạo giao diện người dùng liên kết với chương trình máy tính viết nhiều ngơn ngữ lập trình khác Với thư viện Toolbox, MATLAB cho phép mơ tính tốn, thực nghiệm nhiều mơ hình thực tế kỹ thuật 1.2.2 Phần mềm Proteus: Proteus Design Suite công cụ phần mềm độc quyền sử dụng chủ yếu cho tự động hóa thiết kế điện tử Phần mềm sử dụng chủ yếu kỹ sư thiết kế điện tử kỹ thuật viên để tạo sơ đồ in điện tử để sản xuất bảng mạch in Bên cạnh Proteus hỗ trợ người dùng mơ mạch hỗ trợ cho việc trình diễn, kiểm chứng, 1.2.3 Phần mềm SolidWorks: SolidWorks phần mềm thiết kế 3D, phần mềm cho phép mô hoạt động vật thể khối vật thể Đặc biệt, phần mềm có khả liên kết phần mềm tính tốn để mơ xác chuyển động vật thể theo hàm toán học, MATLAB 1.2.4 Phần mềm EasyRob: EasyRob công cụ mô robot sử dụng đồ hoạ không gian chiều (3D) hình ảnh hoạt động Một hệ thống 3D-CAD đơn giản cho phép tạo khối hình học khối trụ, khối cầu, khối chữ nhật, khối tam giác, khối hình thang, để vẽ kết cấu robot Trong EasyRob dùng chuột để quay tịnh tiến robot đến tọa độ tuỳ ý EasyRob có chức phóng to, thu nhỏ đối tượng vẽ nhiều phần mềm thiết kế khác… 10 try: spokeCommand = r.recognize_google(audio) print ("read: " + spokeCommand) except sr.UnknownValueError: print ("Problem Listening Try Again") continue except sr.RequestError as e: print ("Error 2") continue print "read a signal" #Ham chung if spokeCommand == 'right': #lay vi tri cuoi va lap lai #baseVal, shoulderVal, elbowVal, wristRotation, wristFlexure, gripVal, speedVal = self.lastCommand.split(',') currentBase = self.base.get() self.base.set(currentBase + 10) command = str(self.base.get()) + ',' + str(self.shoulder.get()) + ',' + str(self.elbow.get()) + ',' + str(self.wRot.get()) + ',' + str(self.wFlex.get()) + ',' + str(self.gripper.get())+ ',' + str(25 - self.theSpeed.get()) + '\n' self.sendCommand2(command) if spokeCommand == 'left': currentBase = self.base.get() self.base.set(currentBase - 10) command = str(self.base.get()) + ',' + str(self.shoulder.get()) + ',' + str(self.elbow.get()) + ',' + str(self.wRot.get()) + ',' + str(self.wFlex.get()) + ',' + str(self.gripper.get())+ ',' + str(25 - self.theSpeed.get()) + '\n' self.sendCommand2(command) #self.move_it() if spokeCommand == 'up': currentShoulder = self.shoulder.get() 64 self.shoulder.set(currentShoulder + 20) command = str(self.base.get()) + ',' + str(self.shoulder.get()) + ',' + str(self.elbow.get()) + ',' + str(self.wRot.get()) + ',' + str(self.wFlex.get()) + ',' + str(self.gripper.get())+ ',' + str(25 - self.theSpeed.get()) + '\n' self.sendCommand2(command) if spokeCommand == 'down': currentShoulder = self.shoulder.get() self.shoulder.set(currentShoulder - 20) command = str(self.base.get()) + ',' + str(self.shoulder.get()) + ',' + str(self.elbow.get()) + ',' + str(self.wRot.get()) + ',' + str(self.wFlex.get()) + ',' + str(self.gripper.get())+ ',' + str(25 - self.theSpeed.get()) + '\n' self.sendCommand2(command) if spokeCommand == 'dance': self.currentSequence = "generalmotion1.txt" self.playback() # Ham khac # thay doi lenh kich hoat chuc nang tu cau lenh if # thay doi ten ham luu lai ten file record if spokeCommand == 'one': self.currentSequence = "one.txt" self.playback() if spokeCommand == 'two': print "front" self.currentSequence = "two.txt" self.playback() if spokeCommand == 'three': print "front" self.currentSequence = "three.txt" self.playback() if spokeCommand == 'four': 65 #Thay doi gia tri va cap nhan GUI print "front" self.currentSequence = "four.txt" self.playback() if spokeCommand == 'five': print "front" self.currentSequence = "five.txt" self.playback() #Gap if spokeCommand == 'clockwise': currentWristRot = self.wRot.get() self.wRot.set(currentWristRot - 20) command = str(self.base.get()) + ',' + str(self.shoulder.get()) + ',' + str(self.elbow.get()) + ',' + str(self.wRot.get()) + ',' + str(self.wFlex.get()) + ',' + str(self.gripper.get())+ ',' + str(25 - self.theSpeed.get()) + '\n' self.sendCommand2(command) if spokeCommand == 'counterclockwise': currentWristRot = self.wRot.get() self.wRot.set(currentWristRot + 20) command = str(self.base.get()) + ',' + str(self.shoulder.get()) + ',' + str(self.elbow.get()) + ',' + str(self.wRot.get()) + ',' + str(self.wFlex.get()) + ',' + str(self.gripper.get())+ ',' + str(25 - self.theSpeed.get()) + '\n' self.sendCommand2(command) if spokeCommand == 'close': currentGrip = self.gripper.get() self.gripper.set(currentGrip - 20) command = str(self.base.get()) + ',' + str(self.shoulder.get()) + ',' + str(self.elbow.get()) + ',' + str(self.wRot.get()) + ',' + str(self.wFlex.get()) + ',' + str(self.gripper.get())+ ',' + str(25 - self.theSpeed.get()) + '\n' self.sendCommand2(command) if spokeCommand == 'open': 66 currentGrip = self.gripper.get() self.gripper.set(currentGrip + 20) command = str(self.base.get()) + ',' + str(self.shoulder.get()) + ',' + str(self.elbow.get()) + ',' + str(self.wRot.get()) + ',' + str(self.wFlex.get()) + ',' + str(self.gripper.get())+ ',' + str(25 - self.theSpeed.get()) + '\n' self.sendCommand2(command) if spokeCommand == 'home': self.goHome() if spokeCommand == 'record': self.recordArmPos() if spokeCommand == 'play': self.playback() if spokeCommand == 'kill': break #++++++++++++++++++++Vong lap chinh++++++++++++++++++++++ root = Tk() root.wm_title("5DOF ARM ROBOTICS - ĐH SPKT VINH LONG") root.configure(background = "#66ff33") img = PhotoImage (file="E:\Desktop\ArmRobot\Data\Logo8.gif") panel = Label(root, image = img) panel.pack(side = "top", fill = "both", expand = "yes") #+++++++++++++++++++++++++++Vong lap chinh+++++++++++++++++ app = Application(root) root.mainloop() 67 • Chương trình mơ MATLAB/Simulink – Solidworks: %Khoa luan tot nghiep %Giao dien dieu khien dong hoc thuan - nghich %GVHD: ThS Mai Nhat Thien %Sinh vien: Chau Trung Tin - Nguyen Tan Thanh function varargout = CONTROL1(varargin) gui_Singleton = 1; gui_State = struct('gui_Name', mfilename, 'gui_Singleton', gui_Singleton, 'gui_OpeningFcn', @CONTROL1_OpeningFcn, 'gui_OutputFcn', @CONTROL1_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 function CONTROL1_OpeningFcn(hObject, eventdata, handles, varargin) handles.output = hObject; guidata(hObject, handles); clc; axes(handles.axes1); imshow('E:\Desktop\ArmRobot\Data\LOGO.png'); function varargout = CONTROL1_OutputFcn(hObject, eventdata, handles) varargout{1} = handles.output; function slider1_Callback(hObject, eventdata, handles) ModelName = 'ROBOT6'; % a1 = 0; a2 = 500; a3 = 420.17; a4 = 160; %lay goc theta1=get(handles.slider1,'value'); set(handles.edit2,'string',num2str(theta1)); 68 theta2=get(handles.slider2,'value'); set(handles.edit3,'string',num2str(theta2)); theta3=get(handles.slider3,'value'); set(handles.edit4,'string',num2str(theta3)); theta4=get(handles.slider4,'value'); set(handles.edit5,'string',num2str(theta4)); %xoay canh tay ben simulink set_param([ModelName '/Slider Gain1'],'Gain',num2str(theta1)); set_param([ModelName '/Slider Gain2'],'Gain',num2str(theta2)); set_param([ModelName '/Slider Gain3'],'Gain',num2str(theta3)); set_param([ModelName '/Slider Gain4'],'Gain',num2str(theta4)); T1 = [ cosd(theta1) sind(theta1) 0 0 -1 -sind(theta1) -cosd(theta1) 0 0; 0; 486; 1]; T2 = [ cosd(theta2) sind(theta2) 0 -sind(theta2) cosd(theta2) 0 0 a2*cosd(theta2); a2*sind(theta2); 0; 1]; T3 = [ cosd(theta3) sind(theta3) 0 -sind(theta3) cosd(theta3) 0 0 a3*cosd(theta3); a3*sind(theta3); 0; 1]; T4 = [ cosd(theta4) -sind(theta4) a4*cosd(theta4); sind(theta4) cosd(theta4) a4*sind(theta4); 0 0; 0 1]; T = T1*T2*T3*T4; px=T(1,4); py=T(2,4); pz=T(3,4); set(handles.edit6,'string',num2str(px)); set(handles.edit7,'string',num2str(py)); set(handles.edit8,'string',num2str(pz)); function slider1_CreateFcn(hObject, eventdata, handles) if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUiCONTROL1BackgroundColor')) set(hObject,'BackgroundColor',[.9 9]); end 69 % khop di chuyen function slider2_Callback(hObject, eventdata, handles) ModelName = 'ROBOT6'; % a1 = 0; a2 = 500; a3 = 420.17; a4 = 160; %lay goc theta1=get(handles.slider1,'value'); set(handles.edit2,'string',num2str(theta1)); theta2=get(handles.slider2,'value'); set(handles.edit3,'string',num2str(theta2)); theta3=get(handles.slider3,'value'); set(handles.edit4,'string',num2str(theta3)); theta4=get(handles.slider4,'value'); set(handles.edit5,'string',num2str(theta4)); %xoay canh tay ben simulink set_param([ModelName '/Slider Gain1'],'Gain',num2str(theta1)); set_param([ModelName '/Slider Gain2'],'Gain',num2str(theta2)); set_param([ModelName '/Slider Gain3'],'Gain',num2str(theta3)); set_param([ModelName '/Slider Gain4'],'Gain',num2str(theta4)); T1 = [ cosd(theta1) sind(theta1) 0 0 -1 -sind(theta1) -cosd(theta1) 0 T2 = [ cosd(theta2) sind(theta2) 0 -sind(theta2) cosd(theta2) 0 0 a2*cosd(theta2); a2*sind(theta2); 0; 1]; T3 = [ cosd(theta3) sind(theta3) 0 -sind(theta3) cosd(theta3) 0 0 a3*cosd(theta3); a3*sind(theta3); 0; 1]; T4 = [ cosd(theta4) sind(theta4) 0 T = T1*T2*T3*T4; -sind(theta4) cosd(theta4) 0 0 a4*cosd(theta4); a4*sind(theta4); 0; 1]; px=T(1,4); 70 0; 0; 486; 1]; py=T(2,4); pz=T(3,4); set(handles.edit6,'string',num2str(px)); set(handles.edit7,'string',num2str(py)); set(handles.edit8,'string',num2str(pz)); function slider2_CreateFcn(hObject, eventdata, handles) if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUiCONTROL1BackgroundColor')) set(hObject,'BackgroundColor',[.9 9]); end function slider3_Callback(hObject, eventdata, handles) ModelName = 'ROBOT6'; % a1 = 0; a2 = 500; a3 = 420.17; a4 = 160; %lay goc theta1=get(handles.slider1,'value'); set(handles.edit2,'string',num2str(theta1)); theta2=get(handles.slider2,'value'); set(handles.edit3,'string',num2str(theta2)); theta3=get(handles.slider3,'value'); set(handles.edit4,'string',num2str(theta3)); theta4=get(handles.slider4,'value'); set(handles.edit5,'string',num2str(theta4)); %xoay canh tay ben simulink set_param([ModelName '/Slider Gain1'],'Gain',num2str(theta1)); set_param([ModelName '/Slider Gain2'],'Gain',num2str(theta2)); set_param([ModelName '/Slider Gain3'],'Gain',num2str(theta3)); set_param([ModelName '/Slider Gain4'],'Gain',num2str(theta4)); T1 = [ cosd(theta1) -sind(theta1) 0; sind(theta1) -cosd(theta1) 0; -1 486; 0 1]; T2 = [ cosd(theta2) sind(theta2) 0 -sind(theta2) cosd(theta2) 0 0 a2*cosd(theta2); a2*sind(theta2); 0; 1]; T3 = [ cosd(theta3) sind(theta3) 0 -sind(theta3) cosd(theta3) 0 0 a3*cosd(theta3); a3*sind(theta3); 0; 1]; 71 T4 = [ cosd(theta4) -sind(theta4) a4*cosd(theta4); sind(theta4) cosd(theta4) a4*sind(theta4); 0 0; 0 1]; T = T1*T2*T3*T4; px=T(1,4); py=T(2,4); pz=T(3,4); set(handles.edit6,'string',num2str(px)); set(handles.edit7,'string',num2str(py)); set(handles.edit8,'string',num2str(pz)); function slider3_CreateFcn(hObject, eventdata, handles) if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUiCONTROL1BackgroundColor')) set(hObject,'BackgroundColor',[.9 9]); end function slider4_Callback(hObject, eventdata, handles) ModelName = 'ROBOT6'; % a1 = 0; a2 = 500; a3 = 420.17; a4 = 160; %lay goc xoay theta1=get(handles.slider1,'value'); set(handles.edit2,'string',num2str(theta1)); theta2=get(handles.slider2,'value'); set(handles.edit3,'string',num2str(theta2)); theta3=get(handles.slider3,'value'); set(handles.edit4,'string',num2str(theta3)); theta4=get(handles.slider4,'value'); set(handles.edit5,'string',num2str(theta4)); %xoay canh tay ben simulink set_param([ModelName '/Slider Gain1'],'Gain',num2str(theta1)); set_param([ModelName '/Slider Gain2'],'Gain',num2str(theta2)); set_param([ModelName '/Slider Gain3'],'Gain',num2str(theta3)); set_param([ModelName '/Slider Gain4'],'Gain',num2str(theta4)); T1 = [ cosd(theta1) -sind(theta1) 0; sind(theta1) -cosd(theta1) 0; -1 486; 0 1]; T2 = [ cosd(theta2) -sind(theta2) sind(theta2) cosd(theta2) 72 0 a2*cosd(theta2); a2*sind(theta2); 0 0 0; 1]; T3 = [ cosd(theta3) sind(theta3) 0 -sind(theta3) cosd(theta3) 0 0 a3*cosd(theta3); a3*sind(theta3); 0; 1]; T4 = [ cosd(theta4) sind(theta4) 0 T = T1*T2*T3*T4; -sind(theta4) cosd(theta4) 0 0 a4*cosd(theta4); a4*sind(theta4); 0; 1]; px=T(1,4); py=T(2,4); pz=T(3,4); set(handles.edit6,'string',num2str(px)); set(handles.edit7,'string',num2str(py)); set(handles.edit8,'string',num2str(pz)); function slider4_CreateFcn(hObject, eventdata, handles) if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUiCONTROL1BackgroundColor')) set(hObject,'BackgroundColor',[.9 9]); end function edit1_Callback(hObject, eventdata, handles) function edit1_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUiCONTROL1BackgroundColor')) set(hObject,'BackgroundColor','white'); end function edit2_Callback(hObject, eventdata, handles) function edit2_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUiCONTROL1BackgroundColor')) 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,'defaultUiCONTROL1BackgroundColor')) 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,'defaultUiCONTROL1BackgroundColor')) set(hObject,'BackgroundColor','white'); 73 end function edit5_Callback(hObject, eventdata, handles) function edit5_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUiCONTROL1BackgroundColor')) set(hObject,'BackgroundColor','white'); end function edit6_Callback(hObject, eventdata, handles) function edit6_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUiCONTROL1BackgroundColor')) set(hObject,'BackgroundColor','white'); end function edit7_Callback(hObject, eventdata, handles) function edit7_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUiCONTROL1BackgroundColor')) set(hObject,'BackgroundColor','white'); end function edit8_Callback(hObject, eventdata, handles) function edit8_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUiCONTROL1BackgroundColor')) set(hObject,'BackgroundColor','white'); end function pushbutton2_Callback(hObject, eventdata, handles) ModelName = 'ROBOT6'; % a1 = 486; a2 = 500; a3 = 420.17; a4 = 160; theta1 = 0; theta2 = 0; theta3 = 0; theta4 = 0; %xoay canh tay ben simulink set_param([ModelName '/Slider Gain1'],'Gain',num2str(theta1)); set_param([ModelName '/Slider Gain2'],'Gain',num2str(theta2)); set_param([ModelName '/Slider Gain3'],'Gain',num2str(theta3)); set_param([ModelName '/Slider Gain4'],'Gain',num2str(theta4)); T1 = [ cosd(theta1) -sind(theta1) 0; sind(theta1) -cosd(theta1) 0; -1 486; 74 0 1]; T2 = [ cosd(theta2) sind(theta2) 0 -sind(theta2) cosd(theta2) 0 0 a2*cosd(theta2); a2*sind(theta2); 0; 1]; T3 = [ cosd(theta3) sind(theta3) 0 -sind(theta3) cosd(theta3) 0 0 a3*cosd(theta3); a3*sind(theta3); 0; 1]; T4 = [ cosd(theta4) -sind(theta4) a4*cosd(theta4); sind(theta4) cosd(theta4) a4*sind(theta4); 0 0; 0 1]; T = T1*T2*T3*T4; px=T(1,4); py=T(2,4); pz=T(3,4); set(handles.slider1,'value',theta1); set(handles.slider2,'value',theta2); set(handles.slider3,'value',theta3); set(handles.slider4,'value',theta4); set(handles.edit2,'string',num2str(0)); set(handles.edit3,'string',num2str(0)); set(handles.edit4,'string',num2str(0)); set(handles.edit5,'string',num2str(0)); set(handles.edit6,'string',num2str(px)); set(handles.edit7,'string',num2str(py)); set(handles.edit8,'string',num2str(pz)); function RUN_Callback(hObject, eventdata, handles) ModelName = 'ROBOT6'; % Mo mo phong simulink open_system(ModelName); set_param(ModelName,'BlockReduction','off'); set_param(ModelName,'StopTime','inf'); set_param(ModelName,'simulationMode','normal'); set_param(ModelName,'StartFcn','1'); set_param(ModelName, 'SimulationCommand', 'start'); function STOP_Callback(hObject, eventdata, handles) close; function INVERSE_Callback(hObject, eventdata, handles) ModelName = 'ROBOT6'; global var; px=get(handles.slider5,'value'); set(handles.edit9,'string',num2str(px)); py=get(handles.slider6,'value'); 75 set(handles.edit10,'string',num2str(py)); pz=get(handles.slider7,'value'); set(handles.edit11,'string',num2str(pz)); set(handles.edit6,'string',num2str(px)); set(handles.edit7,'string',num2str(py)); set(handles.edit8,'string',num2str(pz)); a1 = 486; a2 = 500; a3 = 420.17; a4 = 160; theta1=atan2d(py,px); theta234= 0; d=sqrt(px^2+py^2);%DB = OA xd=d*cosd(theta1); yd=d*sind(theta1); r4=d-a4*cosd(theta234); z4=pz-a4*sind(theta234);% s=sqrt((z4-a1)^2+r4^2); theta3=acosd((s^2-a2^2-a3^2)/(2*a2*a3)); beta=atan2d(a3*sind(theta3),a2+a3*cosd(theta3)); alpha=atan2d(z4-a1,r4); theta2=alpha+beta; theta4=theta234-theta2-theta3; guidata(hObject,handles); set_param([ModelName '/Slider Gain1'],'Gain',num2str(theta1)); set_param([ModelName '/Slider Gain2'],'Gain',num2str(theta2)); set_param([ModelName '/Slider Gain3'],'Gain',num2str(theta3)); set_param([ModelName '/Slider Gain4'],'Gain',num2str(theta4)); set(handles.edit2,'string',num2str(theta1)); set(handles.edit3,'string',num2str(theta2)); set(handles.edit4,'string',num2str(theta3)); set(handles.edit5,'string',num2str(theta4)); function slider5_Callback(hObject, eventdata, handles) px=get(handles.slider5,'value'); set(handles.edit9,'string',num2str(px)); function slider5_CreateFcn(hObject, eventdata, handles) if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUiCONTROL1BackgroundColor')) set(hObject,'BackgroundColor',[.9 9]); end function slider6_Callback(hObject, eventdata, handles) py=get(handles.slider6,'value'); set(handles.edit10,'string',num2str(py)); function slider6_CreateFcn(hObject, eventdata, handles) 76 if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUiCONTROL1BackgroundColor')) set(hObject,'BackgroundColor',[.9 9]); end function slider7_Callback(hObject, eventdata, handles) pz=get(handles.slider7,'value'); set(handles.edit11,'string',num2str(pz)); function slider7_CreateFcn(hObject, eventdata, handles) if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUiCONTROL1BackgroundColor')) set(hObject,'BackgroundColor',[.9 9]); end function edit9_Callback(hObject, eventdata, handles) function edit9_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUiCONTROL1BackgroundColor')) set(hObject,'BackgroundColor','white'); end function edit10_Callback(hObject, eventdata, handles) function edit10_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUiCONTROL1BackgroundColor')) set(hObject,'BackgroundColor','white'); end function edit11_Callback(hObject, eventdata, handles) function edit11_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUiCONTROL1BackgroundColor')) set(hObject,'BackgroundColor','white'); end function edit13_Callback(hObject, eventdata, handles) function edit13_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUiCONTROL1BackgroundColor')) set(hObject,'BackgroundColor','white'); end function edit14_Callback(hObject, eventdata, handles) function edit14_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUiCONTROL1BackgroundColor')) set(hObject,'BackgroundColor','white'); end function edit15_Callback(hObject, eventdata, handles) function edit15_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUiCONTROL1BackgroundColor')) set(hObject,'BackgroundColor','white'); end 77 function edit16_Callback(hObject, eventdata, handles) function edit16_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUiCONTROL1BackgroundColor')) set(hObject,'BackgroundColor','white'); end function edit17_Callback(hObject, eventdata, handles) function edit17_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUiCONTROL1BackgroundColor')) set(hObject,'BackgroundColor','white'); end function edit18_Callback(hObject, eventdata, handles) function edit18_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function edit19_Callback(hObject, eventdata, handles) function edit19_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function edit21_Callback(hObject, eventdata, handles) function edit21_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function edit22_Callback(hObject, eventdata, handles) function edit22_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end 78