You are on page 1of 28

BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021

GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa

MÔ HÌNH HÓA ROBOT RRR


1. Cài đặt Matlab 2017b
• Link download Simscape Multibody Link: ‘smlink.r2017b.win64.zip’. Lưu ý: Chọn phiên
bản phù hợp với phiên bản Matlab.
https://www.mathworks.com/campaigns/offerings/download_smlink_confirmation.
html
• Cài đặt add-in Simscape vào Matlab (phải Run as administrator phần mềm Matlab,
nếu không thì khi chạy install_addon.m sẽ báo lỗi Access denied):
install_addon('smlink.r2017b.win64.zip')
2. Cài đặt SolidWorks 2017
• Addin Simscape: https://www.youtube.com/watch?v=7-fkk7DU9fA&t=268s
3. Các lệnh cơ bản trong SolidWorks
• Extrude: tạo khối đùn từ một tiết diện.
• Revolve: tạo khối tròn xoay từ một tiết diện.

• Insert/move
• Đọc, tạo, xuất bản vẽ. ft
• Reference: tạo các tham chiếu: point, face, line

Để luyện tập thiết kế robot trên SolidWorks, sinh viên có thể tham khảo khoá học
a
SolidWorks: Design for Mechatronics của Justin Flett tại LinkedLearning:
https://www.linkedin.com/learning/solidworks-design-for-mechatronics/
Hoặc xem các video hướng dẫn tại đây. Lưu ý khoá học trên sử dụng SolidWorks 2018 nên
dr
các file .sldprt của động cơ bước Nema 17 có thể không tương thích với phiên bản
SolidWorks mà sinh viên sử dụng.
4. Các lệnh cơ bản trong Matlab
• Kiểu dữ liệu trong Matlab
• Tính toán ma trận
• Hàm solve()
• Hàm subs()
• Khối chuyển đổi tín hiệu: Simulink-PS Converter, PS-Simulink Converter
• Khối Slider gain
• Khối nguồn Step
• Chuyển đổi mô hình từ SolidWorks sang Matlab
• Mô phỏng cánh tay trên Matlab
• Giao diện điều khiển GUI
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa

5. Xây dựng mô hình robot RRR


5.1. Bảng tham số D-H và các ma trận
Bài toán ví dụ: Mô hình hóa một cánh tay robot 3 bậc tự do RRR như sau:

Hình 1. Robot RRR


Ví dụ này được lấy từ bài tập 6, tài liệu tham khảo Robot công nghiệp – Phạm Đăng Phước –
2007, chương 3. Robot gồm 4 khâu, 3 khâu động và 1 khâu chuẩn, tay gắp chấp hành gắn cứng
vào khâu 3, ba khớp quay RRR.

Nhắc lại ma trận biến đổi thuần nhất mô tả hướng và vị trí của hệ tọa độ gắn trên khâu thứ 𝑖 so
a ft
với hệ tọa độ gắn trên khâu thứ 𝑖 − 1:
𝐴𝑖 = 𝑅𝑜𝑡(𝑧𝑖−1 , 𝜃𝑖 ) × 𝑇𝑟𝑎𝑛𝑠(0,0, 𝑑𝑖 ) × 𝑇𝑟𝑎𝑛𝑠(𝑎𝑖 , 0,0) × 𝑅𝑜𝑡(𝑥𝑖 , 𝛼𝑖 )
dr

Hình 2. Minh hoạ việc chuyển đổi HTĐ gắn vào khâu thứ 𝑖 so với HTĐ gắn vào khâu thứ 𝑖 − 1
Thứ tự các phép biến đổi:
▪ Quay quanh 𝑧𝑖−1 1 góc 𝜃𝑖
▪ Tịnh tiến dọc theo 𝑧𝑖−1 1 đoạn 𝑑𝑖
▪ Tịnh tiến dọc theo 𝑥𝑖−1 1 đoạn 𝑎𝑖
▪ Quay quanh 𝑥𝑖−1 (≡ 𝑥𝑖 ) 1 góc 𝛼𝑖
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa

Thiết lập các tọa độ đối với robot RRR:

(a) Minh hoạ trong mặt phẳng 2D (b) Minh hoạ 3D (có vấn đề trên hình vẽ)
Hình 3. Thiết lập các hệ toạ độ cho robot

Bảng 1. Bảng tham số D-H


a ft Khâu 𝜽𝒊 𝜶𝒊 𝒂𝒊

3
𝜃1 90

𝜃2

𝜃3
0

0
0

𝑎2

𝑎3
𝒅𝒊

𝑑1

0
dr
Các ma trận biến đổi thuần nhất 𝐴𝑖 :
𝐴1 = 𝑅𝑜𝑡(𝑧0 , 𝜃1 ) × 𝑇𝑟𝑎𝑛𝑠(0,0, 𝑑1 ) × 𝑅𝑜𝑡(𝑋1 , 𝛼1 )
𝐴2 = 𝑅𝑜𝑡(𝑧1 , 𝜃2 ) × 𝑇𝑟𝑎𝑛𝑠(𝑎2 , 0,0)
𝐴3 = 𝑅𝑜𝑡(𝑧2 , 𝜃3 ) × 𝑇𝑟𝑎𝑛𝑠(𝑎3 , 0,0)
Ma trận biến đổi thuần nhất của hệ toạ độ gắn trên khâu chấp hành cuối của tay máy so với hệ
tọa độ gốc:
𝑇 = 𝐴1 ∗ 𝐴2 ∗ 𝐴3
Kết quả tính trong Matlab:

Nếu khâu chấp hành cuối của tay máy được gắn một hệ tọa độ như Hình 4:
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa

Hình 4. Hệ tọa độ đặt vào khâu chấp hành cuối của tay máy
Với:
𝑛𝑥 𝑜𝑥 𝑎𝑥
𝑅 = [𝑛𝑦 𝑜𝑦 𝑎𝑦 ] là ma trận cosin chỉ hướng của hệ {𝑂3} so với hệ {O}
𝑛𝑧 𝑜𝑧 𝑎𝑧
𝑝𝑥
𝑝
𝒑 = [ 𝑦 ] là vector tọa độ tâm tay gắp 𝑂3 trong hệ tọa độ gốc.
𝑝𝑧
Khi đó ma trận 𝑇 có dạng:
a ft 𝑛𝑥
𝑛
𝑇=[ 𝑦
𝑛𝑧
0
𝑜𝑥
𝑜𝑦
𝑜𝑧
0
𝑎𝑥 𝑝𝑥
𝑎𝑦 𝑝𝑦
𝑎𝑧 𝑝𝑧
0 1
]
dr
5.2. Bài toán động học thuận, ngược
• Điều kiện ràng buộc các biến khớp: hạn chế bài toán đa nghiệm.
−180 < 𝜃1 ≤ 180
0 ≤ 𝜃2 ≤ 90
−180 ≤ 𝜃3 ≤ 0
• Bài toán động học thuận: cho giá trị 3 biến khớp 𝜃1 , 𝜃2 , 𝜃3 tìm tọa độ vector p.
cos 𝜃1 (𝑎3 cos(𝜃2 + 𝜃3 ) + 𝑎2 cos 𝜃2 )
𝒑 = [ sin 𝜃1 (𝑎3 cos(𝜃2 + 𝜃3 ) + 𝑎2 cos 𝜃2 ) ]
𝑑1 + 𝑎3 sin(𝜃2 + 𝜃3 ) + 𝑎2 sin 𝜃2
• Bài toán động học ngược: cho tọa độ vector p tìm giá trị 3 biến khớp 𝜃1 , 𝜃2 , 𝜃3 .
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa

Hình 5. Hình chiếu vector p lên 3 trục tọa độ gốc


Tìm 𝜽𝟏
a ft
Nếu đã biết trước tọa độ vector p, ta hoàn toàn có thể xác định được:

cos 𝜃1 =

sin 𝜃1 =
𝑝𝑥
√𝑝𝑥2 + 𝑝𝑦2
𝑝𝑦
√𝑝𝑥2 + 𝑝𝑦2
dr
𝑝𝑦
tan 𝜃1 =
𝑝𝑥
Chọn nghiệm 𝜃1 dựa vào hình sau:
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa

Hình 6. Chọn nghiệm 𝜃1


Tìm 𝜽𝟐 , 𝜽𝟑
a
Ta được hệ phương trình:
𝑝𝑥

𝑝𝑥 =
ft
cos 𝜃1 (𝑎3 cos(𝜃2 + 𝜃3 ) + 𝑎2 cos 𝜃2 )
𝒑 = [𝑝𝑦 ] = [ sin 𝜃1 (𝑎3 cos(𝜃2 + 𝜃3 ) + 𝑎2 cos 𝜃2 ) ]
𝑝𝑧 𝑑1 + 𝑎3 sin(𝜃2 + 𝜃3 ) + 𝑎2 sin 𝜃2

𝑝𝑥
√𝑝𝑥2+ 𝑝𝑦2
(𝑎3 cos(𝜃2 + 𝜃3 ) + 𝑎2 cos 𝜃2 )
dr
𝑝𝑦
𝑝𝑦 = (𝑎3 cos(𝜃2 + 𝜃3 ) + 𝑎2 cos 𝜃2 )
√𝑝𝑥2 + 𝑝𝑦2
{ 𝑝𝑧 = 𝑑1 + 𝑎3 sin(𝜃2 + 𝜃3 ) + 𝑎2 sin 𝜃2

𝑎3 cos(𝜃2 + 𝜃3 ) + 𝑎2 cos 𝜃2 = √𝑝𝑥2 + 𝑝𝑦2


{
𝑑1 + 𝑎3 sin(𝜃2 + 𝜃3 ) + 𝑎2 sin 𝜃2 = 𝑝𝑧
Có thể giải hệ phương trình trên bằng lệnh solve trong Matlab. Cho ra 2 cặp nghiệm, chọn
nghiệm có 𝜃3 < 0.
Hoặc dùng phương pháp giải tích. Chuyển về hệ tọa độ ZOR.
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa

Hình 7. Chuyển về hệ toạ độ ZOR

𝑝𝑟 = √𝑝𝑥2 + 𝑝𝑦2
a {
ft
02 là giao điểm giữa đường tròn 𝑐1 tâm 𝑂1 bán kính 𝑎2 và đường tròn 𝑐2 tâm 𝑂3 bán kính 𝑎3 :
𝑟 2 + (𝑧 − 𝑑1 )2 = 𝑎22
(𝑟 − 𝑝𝑟 )2 + (𝑧 − 𝑝𝑧 )2 = 𝑎32
Hệ này cho ra hai nghiệm 𝑂2 (𝑟2 ; 𝑧2 ) và 𝑂′2 (𝑟′2 ; 𝑧′2 )
Ta có:
dr
𝑟2
cos 𝜃2 =
𝑎2
𝑝𝑟 − 𝑟2
cos 𝛽 =
𝑎3
𝜃3 = −(𝜃2 + 𝛽)

%khai bao symbolic variables cac bien


syms t1 t2 t3 a2 a3 d1 px py pz;
%them cac dieu kien so thuc
assume(t1,'real');
assume(t2,'real');
assume(t3,'real');
assume(px,'real');
assume(py,'real');
assume(pz,'real');
assume(d1,'real');assume(d1>0);
assume(a2,'real');assume(a2>0);
assume(a3,'real');assume(a3>0);
%tinh toan cac ma tran
A1=Rotz(t1)*Trans(0,0,d1)*Rotx(90);
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa

A2=Rotz(t2)*Trans(a2,0,0);
A3=Rotz(t3)*Trans(a3,0,0);
%ma tran bien doi thuan nhat
T=A1*A2*A3;
%don gian hoa cac bieu thuc
T=simplify(T)
%trich phan tu cot 4
p=T(:,4)

Trong đó định nghĩa các hàm sau:

function y = Rotx(t)
%phep quay quanh truc x
y=[1 0 0 0;0 cosd(t) -sind(t) 0; 0 sind(t) cosd(t) 0;0 0 0 1];
end

function y = Rotz(t)
%phep quay quanh truc z
y=[cos(t) -sin(t) 0 0; sin(t) cos(t) 0 0;0 0 1 0;0 0 0 1];
end

function a = Trans(x,y,z)
a
%phep tinh tien theo truc x, y, z
ft
a=[1 0 0 x;0 1 0 y; 0 0 1 z; 0 0 0 1]; %trong bai toan robot y = 0
end
dr
5.3. Bản vẽ các khâu robot
Độ dài các khâu được cho như sau:
Bảng 2. Bảng thông số các khâu của robot

Khâu 𝜽𝒊 𝜶𝒊 𝒂𝒊 𝒅𝒊

1 𝜃1 90 0 𝑑1 = 180𝑚𝑚

2 𝜃2 0 𝑎2 = 250𝑚𝑚 0

3 𝜃3 0 𝑎3 = 200𝑚𝑚 0
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa

a ft
dr
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa

a ft
dr
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa

a ft
dr
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa

a ft
dr
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa

a ft
dr
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa

6. Chuyển đổi sang Matlab


6.1. SimMechanics
• Xuất file SolidWorks sang file .xml ví dụ “robot_RRR.xml”
• Chuyển mô hình vào Matlab, dùng lệnh:
smlink_linksw;
smimport('robot_RRR.xml');

6.2. Giao diện điều khiển


a ft
dr

slider1 edit1 slider4 edit4

slider2 edit2 slider5 edit5

slider3 edit3 slider6 edit6

pushbutton1 pushbutton3 pushbutton4 pushbutton2

Trong đó:
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa

• Các tiêu đề theta1, theta2, theta3, px, py, pz là các Slider gain từ 1-6. Khi dịch chuyển
trên thanh siler gain, giá trị của nó sẽ thay đổi. Các slider gain có giá trị là “num”.
Tác vụ 1: Di chuyển slider1/2/3 → xuất giá trị cho edit1/2/3
Tác vụ 2: Di chuyển slider4/5/6 → xuất giá trị cho edit4/5/6
• 6 edit text từ 1-6 ngoài chức năng hiển thị, còn có chức năng lấy giá trị từ bàn phím.
Kiểu dữ liệu là “string”.
Tác vụ 3: Nhập giá trị edit1/2/3 → xuất giá trị cho slider1/2/3
Tác vụ 4: Nhập giá trị edit4/5/6 → xuất giá trị cho slider4/5/6
• Các tiêu đề solve1, solve2, reset, close là các nút nhấn pushbutton từ 1-4. Khi kích hoạt
nút nhấn, các tác vụ sẽ chạy trong hàm của nó.
Tác vụ 5: Nhấn pushbutton1 → lấy giá tri từ silder1/2/3 → giải bài toán động học thuận → xuất
giá trị cho edit4/5/6 → xuất giá trị cho slider4/5/6 → xuất giá trị cho gain/gain1/gain2 → chạy
mô phỏng trên SimMechanics.
Tác vụ 6: Nhấn pushbutton2 → lấy giá trị từ silder4/5/6 → giải bài toán động học nghịch →




ft
xuất giá trị cho edit1/2/3 → xuất giá trị cho slider1/2/3 → gán giá trị cho gain/gain1/gain2 →
chạy mô phỏng trên SimMechanics.
Tác vụ 7: Nhấn pushbutton3→ reset slider1/2/3 edit1/2/3 về 0 → gọi lại hàm pushbutton1
a
Tác vụ 8: Nhấn pushbutton4→ gọi hàm “close”
Bài toán động học thuận: tác vụ 1 hoặc 3 → tác vụ 5.
Bài toán động học nghịc: tác vụ 2 hoặc 4 → tác vụ 6.
Vị trí ban đầu → tác vụ 7.
dr
• Đóng cửa sổ → tác vụ 8.
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa

6.3. Code Matlab

function varargout = gui(varargin)


gui_Singleton = 1;
gui_State = struct('gui_Name', mfilename, ...
'gui_Singleton', gui_Singleton, ...
'gui_OpeningFcn', @gui_OpeningFcn, ...
'gui_OutputFcn', @gui_OutputFcn, ...
'gui_LayoutFcn', [] , ...
'gui_Callback', []);
if nargin && ischar(varargin{1})
gui_State.gui_Callback = str2func(varargin{1});
end

if nargout

gui_mainfcn(gui_State, varargin{:});
end
a ft
[varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
else
dr
function gui_OpeningFcn(hObject, eventdata, handles, varargin)
% This function has no output args, see OutputFcn.
% hObject handle to figure
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% varargin command line arguments to gui (see VARARGIN)
handles.output = hObject;
%cac dieu kien dau cho slider gain(neu can)
%set(handles.slider1, 'Min', -180);
%set(handles.slider1, 'Max', 180);
%set(handles.slider1, 'SliderStep', [10/360 , 10/360 ]);
%set(handles.slider2, 'Min', -90);
%set(handles.slider2, 'Max', 180);
%set(handles.slider2, 'SliderStep', [10/270 , 10/270 ]);
%set(handles.slider3, 'Min', -180);
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa

%set(handles.slider3, 'Max', 0);


%set(handles.slider3, 'SliderStep', [10/180 , 10/180 ]);
% Update handles structure
guidata(hObject, handles);

function varargout = gui_OutputFcn(hObject, eventdata, handles)


% varargout cell array for returning output args (see VARARGOUT);
% hObject handle to figure
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Get default command line output from handles structure
varargout{1} = handles.output;

a
% hObject handle to slider1 (see GCBO)
ft
function slider1_Callback(hObject, eventdata, handles)

% eventdata reserved - to be defined in a future version of MATLAB


% handles structure with handles and user data (see GUIDATA)
Tác vụ 1.1
dr
% Hints: get(hObject,'Value') returns position of slider
% get(hObject,'Min') and get(hObject,'Max') to determine range of slider
a = get(hObject,'value');%lay gia tr? slider1
set(handles.edit1,'string',num2str(a));%xuat gia tri cho edit1

function slider1_CreateFcn(hObject, eventdata, handles)


% hObject handle to slider1 (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 .9]);
end
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa

function edit1_Callback(hObject, eventdata, handles)


Tác vụ 3.1
% hObject handle to edit1 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
a=get(handles.edit1,'string');%lay gia tri tu edit1
set(handles.slider1,'value',str2num(a));%xuat gia tri cho slider1
% Hints: get(hObject,'String') returns contents of edit1 as text
% str2double(get(hObject,'String')) returns contents of edit1 as a double

function edit1_CreateFcn(hObject, eventdata, handles)


% hObject handle to edit1 (see GCBO)

%
a ft
% 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.
dr
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end

function edit2_Callback(hObject, eventdata, handles)


% hObject handle to edit1 (see GCBO)
Tác vụ 3.2
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
a=get(handles.edit2,'string');%lay gia tri edit2
set(handles.slider2,'value',str2num(a));%xuat gia tri cho slider2

function edit2_CreateFcn(hObject, eventdata, handles)


BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa

% 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 slider2_Callback(hObject, eventdata, handles)


% hObject handle to slider2 (see GCBO)
Tác vụ 1.2
a ft
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
a = get(hObject,'value');%lay gia tri slider2
set(handles.edit2,'string',num2str(a));%xuat gia tri cho edit2
% Hints: get(hObject,'Value') returns position of slider
% get(hObject,'Min') and get(hObject,'Max') to determine range of slider
dr
function slider2_CreateFcn(hObject, eventdata, handles)
% hObject handle to slider2 (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 .9]);
end

function slider3_Callback(hObject, eventdata, handles)


Tác vụ 1.3
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa

% hObject handle to slider3 (see GCBO)


% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
a = get(hObject,'value');%lay gia tri slider3
set(handles.edit3,'string',num2str(a));%xuat gia tri cho edit3

function slider3_CreateFcn(hObject, eventdata, handles)


% hObject handle to slider3 (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'))

end
set(hObject,'BackgroundColor',[.9 .9 .9]);
a ft
function slider4_Callback(hObject, eventdata, handles)
Tác vụ 2.1
dr
% hObject handle to slider3 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
a=get(hObject,'value');%lay gia tri slider4
set(handles.edit4,'string',num2str(a));%xuat gia tri cho edit4
% Hints: get(hObject,'Value') returns position of slider
% get(hObject,'Min') and get(hObject,'Max') to determine range of slider

function slider4_CreateFcn(hObject, eventdata, handles)


% hObject handle to slider3 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns called
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa

% Hint: slider controls usually have a light gray background.


if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor',[.9 .9 .9]);
end

function text4_ButtonDownFcn(hObject, eventdata, handles)


% hObject handle to text4 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)

% --- Executes on slider movement.

% hObject handle to slider5 (see GCBO)

ft
function slider5_Callback(hObject, eventdata, handles)

% eventdata reserved - to be defined in a future version of MATLAB


a
% handles structure with handles and user data (see GUIDATA)
a=get(hObject,'value');%lay gia tri slider5
set(handles.edit5,'string',num2str(a));%xuat gia tri cho edit5
Tác vụ 2.2
dr
function slider5_CreateFcn(hObject, eventdata, handles)
% hObject handle to slider5 (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 .9]);
end

function slider6_Callback(hObject, eventdata, handles)


% hObject handle to slider6 (see GCBO)
Tác vụ 2.3
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa

% eventdata reserved - to be defined in a future version of MATLAB


% handles structure with handles and user data (see GUIDATA)
a=get(hObject,'value');%lay gia tri slider6
set(handles.edit6,'string',num2str(a));%xuat gia tri cho edit6

function slider6_CreateFcn(hObject, eventdata, handles)


% hObject handle to slider6 (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 .9]);
end

% hObject handle to edit2 (see GCBO)


ft
function edit3_Callback(hObject, eventdata, handles)
a
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
Tác vụ 3.3
dr
a=get(handles.edit3,'string');%lay gia tri edit3
set(handles.slider3,'value',str2num(a));%xuat gia tri slider3

function edit3_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
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa

function edit4_Callback(hObject, eventdata, handles)


Tác vụ 4.1
% hObject handle to edit3 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% Hints: get(hObject,'String') returns contents of edit3 as text
% str2double(get(hObject,'String')) returns contents of edit3 as a double
a=get(hObject,'string');%lay gia tri edit4
set(handles.slider4,'value',str2num(a));%xuat gia tri slider4

a ft
function edit4_CreateFcn(hObject, eventdata, handles)
% hObject handle to edit3 (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.
dr
% See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end

function edit5_Callback(hObject, eventdata, handles)


% hObject handle to edit3 (see GCBO) Tác vụ 4.2

% eventdata reserved - to be defined in a future version of MATLAB


% handles structure with handles and user data (see GUIDATA)
a=get(hObject,'string');%lay gia tri edit5
set(handles.slider5,'value',str2num(a));%xuat gia tri cho slider5
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa

function edit5_CreateFcn(hObject, eventdata, handles)


% hObject handle to edit3 (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 edit6_Callback(hObject, eventdata, handles) Tác vụ 4.3


% hObject handle to edit5 (see GCBO)

ft
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
a
a=get(hObject,'string');%lay gia tri edit6
set(handles.slider6,'value',str2num(a));%xuat gia tri cho slider6
dr
function edit6_CreateFcn(hObject, eventdata, handles)
% hObject handle to edit5 (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 pushbutton1_Callback(hObject, eventdata, handles)


% hObject handle to pushbutton1 (see GCBO)
Tác vụ 5
% eventdata reserved - to be defined in a future version of MATLAB
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa

% handles structure with handles and user data (see GUIDATA)


%lay cac gia tri dau vao theta1,theta2,theta3
t1=get(handles.slider1,'value');
t2=get(handles.slider2,'value');
t3=get(handles.slider3,'value');
%xuat gia tri cho cac block gain
set_param('robot_RRR/Slider Gain','Gain',num2str(t1));
set_param('robot_RRR/Slider Gain1','Gain',num2str(t2));
set_param('robot_RRR/Slider Gain2','Gain',num2str(t3));
%tinh toan dong hoc thuan
px=cosd(t1)*(200*cosd(t2 + t3) + 250*cosd(t2));
py=sind(t1)*(200*cosd(t2 + t3) + 250*cosd(t2));
pz=180 + 200*sind(t2 + t3) + 250*sind(t2);
%xuat ket qua cho edit4/5/6
set(handles.edit4,'string',num2str(px));
set(handles.edit5,'string',num2str(py));
set(handles.edit6,'string',num2str(pz));
a
%xuat ket qua cho slider4/5/6
set(handles.slider4,'value',px);
set(handles.slider5,'value',py);
ft
dr
set(handles.slider6,'value',pz);
%chay mo phong ben simulink
%set_param('robot_RRR', 'SimulationCommand', 'stop');
%set_param('robot_RRR', 'SimulationCommand', 'update');
set_param('robot_RRR', 'SimulationCommand', 'start');

function pushbutton2_Callback(hObject, eventdata, handles)


% hObject handle to pushbutton2 (see GCBO)
Tác vụ 6
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
%lay cac gia tri px,py,pz
px=get(handles.slider4,'value');
py=get(handles.slider5,'value');
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa

pz=get(handles.slider6,'value');
%dieu kien xac dinh goc theta 1
if px==0&&py<0
t1=-90;
elseif px==0&&py>=0
t1=90;
elseif py==0&&px>=0
t1=0;
elseif py==0&&px<0
t1=180;
elseif (px>0&&py>0)||(px>0&&py<0)
t1=radtodeg(atan(py/px));
elseif px<0&&py>0
t1=180+radtodeg(atan(py/px));
else t1=-180+radtodeg(atan(py/px));
end
%xac dinh goc theta 2, theta 3
a ft
syms t2 t3;%tao bien hinh thuc symbolic variables
pr=sqrt(px^2+py^2);
ps=sqrt((pz-180)^2+pr^2);
dr
f1=200*cos(t2+t3 )+ 250*cos(t2)-pr;
f2=200*sin(t2+t3)+250*sin(t2)+180-pz;
[t2,t3]=solve(f1,f2,t2,t3);%dung solve giai bai toan dong hoc nghich
t=[t2/pi*180 t3/pi*180];%chuyen qua radian, ma tran t chua 2 cap nghiem
t=double(t);%dua ve dang so thap phan double
%dieu kien chon nghiem theta 2,theta3
if ps==450||(t(1,1)>t(2,1))
t2=t(1,1);
t3=t(1,2);
else t2=t(2,1);t3=t(2,2);
end
%xuat gia tri cho edit1/2/3
set(handles.edit1,'string',num2str(t1));
set(handles.edit2,'string',num2str(t2));
set(handles.edit3,'string',num2str(t3));
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa

%goi lai ham edit1/2/3


edit1_Callback(@edit1_Callback, eventdata, handles);
edit2_Callback(@edit2_Callback, eventdata, handles);
edit3_Callback(@edit3_Callback, eventdata, handles);
%xuat gia tri cho cac block gain ben simulink
set_param('robot_RRR/Slider Gain','Gain',num2str(t1));
set_param('robot_RRR/Slider Gain1','Gain',num2str(t2));
set_param('robot_RRR/Slider Gain2','Gain',num2str(t3));
%mo phong simulink
set_param('robot_RRR', 'SimulationCommand', 'start');

function pushbutton3_Callback(hObject, eventdata, handles)


a
%xuat gia tri cho cac edit1/2/3 ve 0
set(handles.edit1,'string',0);
set(handles.edit2,'string',0);
ft
% hObject handle to pushbutton3 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)

Tác vụ 7
dr
set(handles.edit3,'string',0);
%goi lai cac ham edit1/2/3
edit1_Callback(@edit1_Callback, eventdata, handles);
edit2_Callback(@edit2_Callback, eventdata, handles);
edit3_Callback(@edit3_Callback, eventdata, handles);
%goi lai ham pushbutton1(solve1)
pushbutton1_Callback(@pushbutton1_Callback, eventdata, handles);
%update lai simulink
%set_param('robot_RRR', 'SimulationCommand', 'stop');
set_param('robot_RRR', 'SimulationCommand', 'update');
%set_param('robot_RRR', 'SimulationCommand', 'start');
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa

function pushbutton4_Callback(hObject, eventdata, ~)


% hObject handle to pushbutton4 (see GCBO) Tác vụ 8
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
close;

a ft
dr

You might also like