P a g e 55 | 108
P a g e 56 | 108
CHƯƠNG 5. THI CƠNG MƠ PHỎNG MƠ HÌNH 5.1. Cách điều khiển.
❖ Nhấn nút Run để bắt đầu chạy và hiện mô phỏng 3D cho robot.
❖ Nhấn nút Defaut cho trở về với các vị trí mà ta đã định trước đó và tính tốn động học thuận.
❖ Nhấn nút Invese để thực hiện phép toán động học nghịch.
❖ Nhấn nút Stop để dừng chương trình Hình 5. 1 Run Hình 5. 2 DEFAUT Hình 5. 3 Inverse Hình 5. 4 Stop Hình 5. 5 Stop
P a g e 57 | 108
❖ Thanh kéo lên xuống Slider bên động học thuận dùng để thay đổi góc quay, và bên động học nghịch dùng để thay đổi vị trí PXZ của toạ độ điểm cuối.
❖ Position để thể hiện vị trí của điểm cuối
Kiểm tra bài tốn động học thuận theo lưu đồ hình, bằng cách trượt các giá trị θ1, θ2, θ3, θ4 vào các ô Theta1, Theta2, Theta3, Theta4 tương ứng ở mục “SliderGain”.
Thanh Position sẽ là các vị trí của khớp cuối cùng. Khi ta có các vị tri X,Y,Z thay lần lượt kéo chỉnh các Px,Py,Pz để ta thực hiện phép tính động học nghịch bằng cách nhấn chọn nút Inverse sau đó sẽ hiển thị các toạ toạ độ của Base, Joint 1, Joint 2, Joint 3. Ta sẽ thơng qua mơ hình chạy trên matlab để xem ta thực hiện đúng hay chưa.
Ta xét trường hợp:
Trường hợp
Giá trị nhập (độ) Giá trị tính tốn được (mm)
𝜃1 𝜃2 𝜃3 𝜃4 x y z
T1 0 0 0 0 1080.17 0 486
T2 0 -90 90 -90 420.17 0 1146
Hình 5. 6 Slider gain
P a g e 58 | 108 Kết quả trường hợp 1: Kết quả trường hợp 2: Kết quả trường hợp 2: Hình 5. 8 Kết quả mơ phỏng T1 Hình 5. 9 Kết quả mơ phỏng T2
P a g e 59 | 108
5.2. Hệ thống điện sử dụng cho tay máy
Ở chương 3 đã tiến hành tính tốn lựa chọn bốn động cơ AC servo tích hợp với hộp giảm tốc Harmonic thuộc hãng HarmonicDrive cho bốn khớp của tay máy.
Để cấp nguồn và điều khiển các động cơ, sử dụng bốn driver cùng hãng cho bốn động cơ này là driver HA-800A-6D/E-200 cho hai động cơ ở khớp 1, 2 của tay máy và driver HA800A-3D/E-200 (hình 5.1) cho hai động cơ ở khớp 3, 4 của tay máy. Các thông số cơ bản của driver được thể hiện trong bảng 5.1.
Lựa chọn chế độ xung cấp cho driver
Xung điều khiển được lựa chọn để cấp cho driver có dạng Open Collector (hình 5.8). Theo đó khi xung được cấp vào chân “Forward command pulse signal input”
Hình 5. 10 HA-800A Series AC Servo Driver
P a g e 60 | 108
động cơ đang được điều khiển sẽ quay thuận và khi xung được cấp vào chân “Reverse command pulse signal input” động cơ đang được điều khiển sẽ quay nghịch.
❖ Card điều khiển
Card PCI - 8134 (hình 5.3) được sử dụng để xuất xung điều khiển cho bốn driver động cơ AC servo trong luận văn. Card PCI - 8134 là card điều khiển chuyển động và có khả năng điều khiển tối đa cho bốn động cơ AC servo. Card gồm có CN1 PIN (External power input) bảng 5.3, CN2 PIN (Main Connector) bảng 5.4, CN3 PIN (Manual pulser input), CN4 PIN (Simultaneous Start/Stop).
❖ Lựa chọn chế độ xung cho card điều khiển:
Jumpers J1 ÷ J8 trên card điều khiển được sử dụng để lựa chọn chế độ xung điều khiển cấp cho driver. Như đã trình bày ở trên, xung điều khiển được lựa chọn để cấp cho driver có dạng Open Collector. Do đó, vị trí các Jumpers được cài đặt như trong hình 5.4.
Hình 5. 12 Card PCI - 8134
P a g e 61 | 108
❖ Cảm biến xác định sản phẩm và pallet:
Có ba cảm biến được sử dụng để phát hiện sản phẩm và pallet. Với các ưu điểm như không tiếp xúc với vật thể cần phát hiện, khơng bị hao mịn, thời gian đáp ứng nhanh, do đó cảm biến quang điện (loại NPN) PNR02 của hãng Hanyoung (hình 5.11) được lựa chọn sử dụng trong luận văn. Cảm biến được cấp nguồn 24VDC, có dịng điện tối đa ở đầu ra là 100 (mA) và có thời gian đáp ứng tối đa là 3(ms). Tín hiệu ở đầu ra (OUT) của cảm biến được nối với chân L1 của 24VDC Relay (hình 5.11) nhằm
điều khiển đóng ngắt relay, cấp tín hiệu cho các chân PEL1, MEL1 và PSD1 của card điều khiển. Các tín hiệu này dùng để báo cho tay máy biết sản phẩm và pallet đã nằm ở đúng vị trí, phục vụ cho quá trình điều khiển tay máy.
Sau khi đã có được các thành phần trong hệ thống điện của tay máy như: động cơ, driver, card điều khiển và các cảm biến như đã trình bày ở trên, tiến hành kết nối các thành phần này lại thành một hệ thống. Trong hình 5.12 là sơ đồ nối dây cho một driver. Các driver còn lại được kết nối tương tự với card điều khiển.
Hình 5. 14 Cảm biến quang điện PN-R02
P a g e 62 | 108
P a g e 63 | 108
CHƯƠNG 6. KẾT QUẢ VÀ ĐÁNH GIÁ 6.1. .Kết quả, phân tích và đánh giá
Cơ học
Nghiên cứu được sự chuyển động cơ học và tác động qua lại lẫn nhau của các khâu từ đó đưa ra phương hướng điều khiển, tính tốn.
Nghiên cứu xác định được vị trí của robot tại thời điểm đang xét, tiến hành được phương pháp điều khiển, cũng như tinh chỉnh.
Nghiên cứu chuyển động tịnh tiến, chuyển động quay, chuyển động song phẳng, từ kết quả nghiên cứu đó chúng ta cũng có thể chọn động cơ cho tay máy sao cho phù hợp và công xuất khả năng làm việc và vận hành tay máy sao cho ổn định khi làm việc, trong cả trạng thái quá tải và điều kiện bình thường.
Lập trình điều khiển
Nhóm đồ án sử dụng phần mềm MATLAB lập trình điều khiển, sử dụng ngơn ngữ lập trình C để thiết kế giao diện điều khiển trên máy tính, với đặc tính được tích hợp rất nhiều ưu điểm trong việc lập trình cũng như hỗ trợ xử lý điều khiển linh hoạt với ứng dụng.
6.2. Hướng phát triển
6.2.1. Đánh giá kết quả đạt được Về phần lý thuyết Về phần lý thuyết
Cần phải nghiên cứu tối ưu hơn bài toán động lực học ngược, và bài toán động lực học thuận để có thể điều khiển độ chĩnh xác về vận tốc và vị trí.
P a g e 64 | 108
Về phần mơ hình
Hạn chế về ý tưởng thiết kế, cần đưa ra nhiều mẫu concept cụ thể để có những cái nhìn khách quan và trực diện hơn về tay máy robot cuối cùng có thể đưa ra một sản phẩm tiêu biểu nhất cho công tác gia công và tạo ra sản phẩm .
6.2.2. Hướng phát triển Về phần lý thuyết: Về phần lý thuyết:
Nghiên cứu chi tiết hơn bài toán động học ngược, bài toán động học thuận để điều khiển động cơ chính xác hơn
Nghiên cứu thêm các thuật tốn nội suy khác: Bresenham, đối xứng, Midpoint,.
Về mơ hình:
Nghiên cứu tối ưu hệ thống cơ khí theo quan điểm của “cơ điện tử”. Cần tối ưu hóa từ phần lên ý tưởng đến phần gia công và cuối cùng là tinh chỉnh cho ra sản phẩm hoàn chỉnh sao cho tổng thời gian là ngắn nhất.
Nghiên cứu trang bị cho tay máy robot thêm các hệ thống cảm biến, hệ thống đinh vị để đạt độ chính xác nhất định.
P a g e 65 | 108
TÀI LIỆU THAM KHẢO
1. TS. Nguyễn Mạnh Tiến, Điều khiển robot công nghiệp, NXB Khoa học Kỹ thuật, 2007.
2. Bùi Thư Cao, Trần Hữu Toàn, Kỹ thuật robot, Đại học Công nghiệp TPHCM, 2008.
3. PGS.TS. Nguyễn Trường Thịnh, Giáo trình Kỹ thuật robot, NXB Đại học
Quốc gia Thành phố Hồ Chí Minh, 2014.
4. TS. Khổng Minh, Giáo trình Robot cơng ngiệp, Tài liệu lưu hành nội bộ,
2016.
5. R.Paul, Robot Manipulators: Mathematics, Programming and Control. MIT Press, Cambridge, MA, 1982.
6. J.Craig, Introduction to Robotics: Mechanics and Control. Addison Wesley, Reading, MA, 1986.
7. Bruno Siciliano, Lorenzo Sciavico, Luigi Villani, Giuseppe Oriolo, Robotics
P a g e 66 | 108
Code mơ hình:
function varargout = Wan(varargin) % WAN MATLAB code for Wan.fig gui_Singleton = 1;
gui_State = struct('gui_Name', mfilename, ... 'gui_Singleton', gui_Singleton, ...
'gui_OpeningFcn', @Wan_OpeningFcn, ... 'gui_OutputFcn', @Wan_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 Wan_OpeningFcn(hObject, eventdata, handles, varargin) handles.output = hObject;
guidata(hObject, handles);
P a g e 67 | 108
varargout{1} = handles.output;
function slider1_Callback(hObject, eventdata, handles) ModelName = 'WAN'; % a1 = 0; a2 = 500; a3 = 420.17; a4 = 160; theta1=get(handles.slider1,'value'); set(handles.edit1,'string',num2str(theta1)); theta2=get(handles.slider2,'value'); set(handles.edit2,'string',num2str(theta2)); theta3=get(handles.slider3,'value'); set(handles.edit3,'string',num2str(theta3)); theta4=get(handles.slider4,'value'); set(handles.edit4,'string',num2str(theta4));
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) 0 -sind(theta1) 0; sind(theta1) 0 -cosd(theta1) 0;
P a g e 68 | 108
0 -1 0 486; 0 0 0 1];
T2 = [ cosd(theta2) -sind(theta2) 0 a2*cosd(theta2); sind(theta2) cosd(theta2) 0 a2*sind(theta2); 0 0 1 0;
0 0 0 1];
T3 = [ cosd(theta3) -sind(theta3) 0 a3*cosd(theta3); sind(theta3) cosd(theta3) 0 a3*sind(theta3); 0 0 1 0;
0 0 0 1];
T4 = [ cosd(theta4) -sind(theta4) 0 a4*cosd(theta4); sind(theta4) cosd(theta4) 0 a4*sind(theta4); 0 0 1 0; 0 0 0 1]; T = T1*T2*T3*T4; px=T(1,4); py=T(2,4); pz=T(3,4); set(handles.edit5,'string',num2str(px));
P a g e 69 | 108
set(handles.edit6,'string',num2str(py)); set(handles.edit7,'string',num2str(pz));
function slider1_CreateFcn(hObject, eventdata, handles)
if isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor',[.9 .9 .9]); end
function edit1_Callback(hObject, eventdata, handles) function edit1_CreateFcn(hObject, eventdata, handles)
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white'); end
function slider2_Callback(hObject, eventdata, handles) ModelName = 'WAN'; % a1 = 0; a2 = 500; a3 = 420.17; a4 = 160;
%get the angle
P a g e 70 | 108 set(handles.edit1,'string',num2str(theta1)); theta2=get(handles.slider2,'value'); set(handles.edit2,'string',num2str(theta2)); theta3=get(handles.slider3,'value'); set(handles.edit3,'string',num2str(theta3)); theta4=get(handles.slider4,'value'); set(handles.edit4,'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) 0 -sind(theta1) 0; sind(theta1) 0 -cosd(theta1) 0; 0 -1 0 486; 0 0 0 1];
T2 = [ cosd(theta2) -sind(theta2) 0 a2*cosd(theta2); sind(theta2) cosd(theta2) 0 a2*sind(theta2); 0 0 1 0;
P a g e 71 | 108
T3 = [ cosd(theta3) -sind(theta3) 0 a3*cosd(theta3); sind(theta3) cosd(theta3) 0 a3*sind(theta3); 0 0 1 0;
0 0 0 1];
T4 = [ cosd(theta4) -sind(theta4) 0 a4*cosd(theta4); sind(theta4) cosd(theta4) 0 a4*sind(theta4); 0 0 1 0; 0 0 0 1]; T = T1*T2*T3*T4; px=T(1,4); py=T(2,4); pz=T(3,4); set(handles.edit5,'string',num2str(px)); set(handles.edit6,'string',num2str(py)); set(handles.edit7,'string',num2str(pz));
function slider2_CreateFcn(hObject, eventdata, handles)
if isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor',[.9 .9 .9]); end
P a g e 72 | 108
function edit2_Callback(hObject, eventdata, handles) function edit2_CreateFcn(hObject, eventdata, handles)
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white'); end
% --- Executes on slider movement.
function slider3_Callback(hObject, eventdata, handles) ModelName = 'WAN'; % a1 = 0; a2 = 500; a3 = 420.17; a4 = 160;
%get the angle
theta1=get(handles.slider1,'value');
set(handles.edit1,'string',num2str(theta1)); theta2=get(handles.slider2,'value');
set(handles.edit2,'string',num2str(theta2)); theta3=get(handles.slider3,'value');
P a g e 73 | 108
set(handles.edit3,'string',num2str(theta3)); theta4=get(handles.slider4,'value');
set(handles.edit4,'string',num2str(theta4));
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) 0 -sind(theta1) 0; sind(theta1) 0 -cosd(theta1) 0; 0 -1 0 486; 0 0 0 1];
T2 = [ cosd(theta2) -sind(theta2) 0 a2*cosd(theta2); sind(theta2) cosd(theta2) 0 a2*sind(theta2); 0 0 1 0;
0 0 0 1];
T3 = [ cosd(theta3) -sind(theta3) 0 a3*cosd(theta3); sind(theta3) cosd(theta3) 0 a3*sind(theta3); 0 0 1 0;
0 0 0 1];
P a g e 74 | 108
T4 = [ cosd(theta4) -sind(theta4) 0 a4*cosd(theta4); sind(theta4) cosd(theta4) 0 a4*sind(theta4); 0 0 1 0; 0 0 0 1]; T = T1*T2*T3*T4; px=T(1,4); py=T(2,4); pz=T(3,4); set(handles.edit5,'string',num2str(px)); set(handles.edit6,'string',num2str(py)); set(handles.edit7,'string',num2str(pz));
function slider3_CreateFcn(hObject, eventdata, handles)
if isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor',[.9 .9 .9]); 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
P a g e 75 | 108
% --- Executes on slider movement.
function slider4_Callback(hObject, eventdata, handles) ModelName = 'WAN'; % a1 = 0; a2 = 500; a3 = 420.17; a4 = 160;
%get the angle
theta1=get(handles.slider1,'value'); set(handles.edit1,'string',num2str(theta1)); theta2=get(handles.slider2,'value'); set(handles.edit2,'string',num2str(theta2)); theta3=get(handles.slider3,'value'); set(handles.edit3,'string',num2str(theta3)); theta4=get(handles.slider4,'value'); set(handles.edit4,'string',num2str(theta4));
%xoay canh tay ben simulink
set_param([ModelName '/Slider Gain1'],'Gain',num2str(theta1)); set_param([ModelName '/Slider Gain2'],'Gain',num2str(theta2));
P a g e 76 | 108
set_param([ModelName '/Slider Gain3'],'Gain',num2str(theta3)); set_param([ModelName '/Slider Gain4'],'Gain',num2str(theta4)); T1 = [ cosd(theta1) 0 -sind(theta1) 0; sind(theta1) 0 -cosd(theta1) 0; 0 -1 0 486; 0 0 0 1];
T2 = [ cosd(theta2) -sind(theta2) 0 a2*cosd(theta2); sind(theta2) cosd(theta2) 0 a2*sind(theta2); 0 0 1 0;
0 0 0 1];
T3 = [ cosd(theta3) -sind(theta3) 0 a3*cosd(theta3); sind(theta3) cosd(theta3) 0 a3*sind(theta3); 0 0 1 0;
0 0 0 1];
T4 = [ cosd(theta4) -sind(theta4) 0 a4*cosd(theta4); sind(theta4) cosd(theta4) 0 a4*sind(theta4); 0 0 1 0;
0 0 0 1]; T = T1*T2*T3*T4;
P a g e 77 | 108 px=T(1,4); py=T(2,4); pz=T(3,4); set(handles.edit5,'string',num2str(px)); set(handles.edit6,'string',num2str(py)); set(handles.edit7,'string',num2str(pz));
function slider4_CreateFcn(hObject, eventdata, handles)
if isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor',[.9 .9 .9]); 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 slider5_Callback(hObject, eventdata, handles) px_inv=get(handles.slider5,'value');
set(handles.edit8,'string',num2str(px_inv));
P a g e 78 | 108
if isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor',[.9 .9 .9]); 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
function slider6_Callback(hObject, eventdata, handles) py_inv=get(handles.slider6,'value');
set(handles.edit9,'string',num2str(py_inv));
function slider6_CreateFcn(hObject, eventdata, handles)
if isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor',[.9 .9 .9]); end
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
P a g e 79 | 108
function slider7_Callback(hObject, eventdata, handles) pz_inv=get(handles.slider7,'value');
set(handles.edit10,'string',num2str(pz_inv));
function slider7_CreateFcn(hObject, eventdata, handles)
if isequal(get(hObject,'BackgroundColor'),
get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor',[.9 .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
function edit8_Callback(hObject, eventdata, handles) function edit8_CreateFcn(hObject, eventdata, handles)
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white'); end
function edit9_Callback(hObject, eventdata, handles) function edit9_CreateFcn(hObject, eventdata, handles).
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
P a g e 80 | 108 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,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white'); end
function pushbutton1_Callback(hObject, eventdata, handles) ModelName = 'WAN'; 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 pushbutton2_Callback(hObject, eventdata, handles) ModelName = 'WAN';
% a1 = 486; a2 = 500;
P a g e 81 | 108 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) 0 -sind(theta1) 0; sind(theta1) 0 cosd(theta1) 0; 0 -1 0 486; 0 0 0 1];
T2 = [ cosd(theta2) -sind(theta2) 0 a2*cosd(theta2); sind(theta2) cosd(theta2) 0 a2*sind(theta2); 0 0 1 0;
P a g e 82 | 108
0 0 0 1];
T3 = [ cosd(theta3) -sind(theta3) 0 a3*cosd(theta3); sind(theta3) cosd(theta3) 0 a3*sind(theta3); 0 0 1 0;
0 0 0 1];
T4 = [ cosd(theta4) -sind(theta4) 0 a4*cosd(theta4); sind(theta4) cosd(theta4) 0 a4*sind(theta4); 0 0 1 0; 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.edit1,'string',num2str(theta1)); set(handles.edit2,'string',num2str(theta2)); set(handles.edit3,'string',num2str(theta3));
P a g e 83 | 108
set(handles.edit4,'string',num2str(theta4)); set(handles.edit5,'string',num2str(px)); set(handles.edit6,'string',num2str(py)); set(handles.edit7,'string',num2str(pz));
function pushbutton3_Callback(hObject, eventdata, handles) ModelName = 'WAN'; global var; px_inv=get(handles.slider5,'value'); set(handles.edit8,'string',num2str(px_inv)); py_inv=get(handles.slider6,'value'); set(handles.edit9,'string',num2str(py_inv)); pz_inv=get(handles.slider7,'value'); set(handles.edit10,'string',num2str(pz_inv)); set(handles.edit5,'string',num2str(px_inv)); set(handles.edit6,'string',num2str(py_inv)); set(handles.edit7,'string',num2str(pz_inv)); L1 = 486; L2 = 500; L3 = 420.17; L4 = 160; %ph??ng pháp ??i s? t1_inv=atan2d(py_inv,px_inv); %theta1 nx=px_inv*cosd(t1_inv)+py_inv*sind(t1_inv)-L4;
P a g e 84 | 108 ny=L1-pz_inv; M=(nx*nx+ny*ny-L2*L2-L3*L3)/(2*L2*L3); %cos(theta3_inv) N=sqrt(1-M*M); %sin(theta3_inv) t3_inv=atan2d(N,M); %theta3 A=ny*(L3*cosd(t3_inv)+L2)-L3*sind(t3_inv)*nx; B=nx*(L3*cosd(t3_inv)+L2)+L3*sind(t3_inv)*ny; t2_inv=atan2d(A,B); %theta2
t4_inv= -t2_inv -t3_inv; %theta4
guidata(hObject,handles);
set_param([ModelName '/Slider Gain1'],'Gain',num2str(t1_inv)); set_param([ModelName '/Slider Gain2'],'Gain',num2str(t2_inv)); set_param([ModelName '/Slider Gain3'],'Gain',num2str(t3_inv)); set_param([ModelName '/Slider Gain4'],'Gain',num2str(t4_inv)); set(handles.edit1,'string',num2str(t4_inv));
set(handles.edit2,'string',num2str(t4_inv));
P a g e 85 | 108
set(handles.edit4,'string',num2str(t4_inv));
function pushbutton4_Callback(hObject, eventdata, handles) close;
function axes4_ButtonDownFcn(hObject, eventdata, handles) handles.output = hObject; guidata(hObject, handles); clc; axes(handles.axes4); imshow('C:\Users\My Poor\Desktop\123\Logohutech.jpg'); axes(handles.axes6); imshow('C:\Users\My Poor\Desktop\123\background.jpg'); function edit12_Callback(hObject, eventdata, handles) function edit12_CreateFcn(hObject, eventdata, handles)
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white'); end