.1 Phần đế robot

Hình 6.3 Thanh nối 2

Hình 6.4 Thanh nối 3

Hình 6.7 Khâu tác động cuối

Sau khi thiết kế từng chi tiết, lắp ráp và đặt tọa độ ta thu được robot MH3BM hồn chỉnh.

Hình 6.8 Robot MH3BM hồn chỉnh

6.2 Kết nối Solid Work và Matlab

Các Toolbox cần có:

▪Simscape Multibody Link: sử dụng cho tùy phiên bản Matlab khác nhau.

▪Thư viện Simscape trong Matlab.

Sau khi cài đặt xong ta liên kết Matlab và Solid work.

Hình 6.9 Thêm Toolbox vào solid work

Sau đó vào mục Tools – Simscape Multibody Link – Export chọn “Simscape Multibody” để tạo file .xml (Trong báo cáo này đặt tên là MH3BM.xml).

Tiếp theo ta chuyển mơ hình đã vẽ vào Simulink bằng cách viết lệnh “smimport(‘MH3BM’)”. Sau khi chuyển đổi hoàn tất ta được mơ hình trong Simulink.

Hình 6.10 Mơ hình trong simulink

6.3 Thiết kế bộ điều khiển PID

Hình 6.11 Mơ hình điều khiển khơng gian khớp của thuật tốn PID

Bảng thông số của bộ PID cho từng khớp như sau:

Bảng 6-1 Thông số PID

Thông số

Kp Ki Kd

Ta thiết kế bộ điều khiển PID trên Matlab cho từng khớp:

Hình 6.12 Bộ PID cho khớp 1

Hình 6.13 Bộ PID cho khớp 2

Hình 6.15 Bộ PID cho khớp 4

Hình 6.16 Bộ PID cho khớp 5

Hình 6.17 Bộ PID cho khớp 6

Hình 6.18 Kết nối bộ điều khiển vào mơ hình động lực học

Sau khi kết nối xong, ta mơ phỏng và thu được kết quả sau:

Hình 6.20 Đáp ứng vị trí khớp 2

Hình 6.21 Đáp ứng vị trí khớp 3

Hình 6.22 Đáp ứng vị trí khớp 4

Hình 6.23 Đáp ứng vị trí khớp 5

CHƯƠNG 7. PHỤ LỤC7.1 Giao diện động học thuận 7.1 Giao diện động học thuận

function varargout

gui_Singleton = 1;

gui_State = struct('gui_Name',

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

% End initialization code - DO NOT EDIT

% --- Executes just before donghocthuan is made visible.

function donghocthuan_OpeningFcn(hObject, eventdata, handles,

varargin)

% This function has no output args, see OutputFcn. % hObject

% eventdata

% handles % varargin

% Choose default command line output for donghocthuan

handles.output = hObject;

% Update handles structure

guidata(hObject, handles);

% UIWAIT makes donghocthuan wait for user response (see UIRESUME)

% uiwait(handles.figure1);

% --- Outputs from this function are returned to the command line.

function varargout = donghocthuan_OutputFcn(hObject, eventdata,

handles)

% varargout cell array for returning output args (see VARARGOUT);

% hObject

% eventdata % handles

% Get default command line output from handles structure

varargout{1} = handles.output;

function nhap_theta1_Callback(hObject, eventdata,

% hObject

% eventdata % handles

% Hints: get(hObject,'String') returns contents of nhap_theta1 as text

% str2double(get(hObject,'String')) returns contents of nhap_theta1 as a double theta1 = str2num(get(handles.nhap_theta1,'string')); if (theta1 > 180) theta1 = 180; end if (theta1 < -180) theta1 = -180; end set(handles.nhap_theta1,'string',num2str(theta1));

% --- Executes during object creation, after setting all properties. function nhap_theta1_CreateFcn(hObject, eventdata, handles)

% hObject

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

% hObject

% eventdata % handles

% Hints: get(hObject,'String') returns contents of nhap_theta6 as text

% str2double(get(hObject,'String')) returns contents of nhap_theta6 as a double theta6 = str2num(get(handles.nhap_theta6,'string')); if (theta6 > 360) theta6 = 360; end if (theta6 < -360) theta6 = -360; end set(handles.nhap_theta6,'string',num2str(theta6));

% --- Executes during object creation, after setting all properties. function nhap_theta6_CreateFcn(hObject, eventdata, handles)

% hObject

% eventdata % handles

% hObject

% eventdata % handles

% Hints: get(hObject,'String') returns contents of nhap_theta5 as text

% str2double(get(hObject,'String')) returns contents of nhap_theta5 as a double theta5 = str2num(get(handles.nhap_theta5,'string')); if (theta5 > 120) theta5 = 120; end if (theta5 < -120) theta5 = -120; end set(handles.nhap_theta5,'string',num2str(theta5));

% --- Executes during object creation, after setting all properties. function nhap_theta5_CreateFcn(hObject, eventdata, handles)

% hObject

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

% hObject

% eventdata % handles

% Hints: get(hObject,'String') returns contents of nhap_theta4 as text

% str2double(get(hObject,'String')) returns contents of nhap_theta4 as a double theta4 = str2num(get(handles.nhap_theta4,'string')); if (theta4 > 170) theta4 = 170; end if (theta4 < -170) theta4 = -170; end set(handles.nhap_theta4,'string',num2str(theta4));

% --- Executes during object creation, after setting all properties. function nhap_theta4_CreateFcn(hObject, eventdata, handles)

% hObject

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

% hObject

% eventdata % handles

% Hints: get(hObject,'String') returns contents of nhap_theta2 as text

% str2double(get(hObject,'String')) returns contents of nhap_theta2 as a double theta2 = str2num(get(handles.nhap_theta2,'string')); if (theta2 > 90) theta2 = 90; end if (theta2 < -85) theta2 = -85; end set(handles.nhap_theta2,'string',num2str(theta2));

% --- Executes during object creation, after setting all properties. function nhap_theta2_CreateFcn(hObject, eventdata, handles)

% hObject

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

% hObject

% eventdata % handles

% Hints: get(hObject,'String') returns contents of nhap_theta3 as text

% str2double(get(hObject,'String')) returns contents of nhap_theta3 as a double theta3 = str2num(get(handles.nhap_theta3,'string')); if (theta3 > 260) theta3 = 260; end if (theta3 < -105) theta3 = -105; end set(handles.nhap_theta3,'string',num2str(theta3));

% --- Executes during object creation, after setting all properties. function nhap_theta3_CreateFcn(hObject, eventdata, handles)

% hObject

% eventdata % handles 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

% --- Executes on button press in Tinh.

function Tinh_Callback(hObject, eventdata, handles)

% hObject % eventdata % handles theta1 = str2num(get(handles.nhap_theta1,'string')); theta2 = str2num(get(handles.nhap_theta2,'string')); theta3 = str2num(get(handles.nhap_theta3,'string')); theta4 = str2num(get(handles.nhap_theta4,'string')); theta5 = str2num(get(handles.nhap_theta5,'string')); theta6 = str2num(get(handles.nhap_theta6,'string')); %%Tinh ma tran T06 a2 = 290; d4 = 270;

A1 = [cosd(theta1) 0 sind(theta1) 0;sind(theta1) 0 -cosd(theta1) 0;0100;0001];

A2 = [cosd(theta2) -sind(theta2) 0 a2*cosd(theta2);sind(theta2) cosd(theta2) 0 a2*sind(theta2);0 0 1 0;0 0 0 1];

A3 = [cosd(theta3) 0 sind(theta3) 0;sind(theta3) 0 -cosd(theta3) 0;0100;0001];

A4 = [cosd(theta4) 0 -sind(theta4) 0;sind(theta4) 0 cosd(theta4) 0;0 -1 0 d4;0 0 0 1];

A5 = [cosd(theta5) 0 sind(theta5) 0;sind(theta5) 0 -cosd(theta5) 0;0100;0001];

A6 = [cosd(theta6) -sind(theta6) 0 0;sind(theta6) cosd(theta6) 0 0;0010;0001]; T = A1*A2*A3*A4*A5*A6; set(handles.T11,'string',num2str(T(1,1))); set(handles.T12,'string',num2str(T(1,2))); set(handles.T13,'string',num2str(T(1,3))); set(handles.T14,'string',num2str(T(1,4))); set(handles.T21,'string',num2str(T(2,1))); set(handles.T22,'string',num2str(T(2,2))); set(handles.T23,'string',num2str(T(2,3))); set(handles.T24,'string',num2str(T(2,4))); set(handles.T31,'string',num2str(T(3,1))); set(handles.T32,'string',num2str(T(3,2))); set(handles.T33,'string',num2str(T(3,3))); set(handles.T34,'string',num2str(T(3,4))); set(handles.T41,'string',num2str(T(4,1))); set(handles.T42,'string',num2str(T(4,2))); set(handles.T43,'string',num2str(T(4,3))); set(handles.T44,'string',num2str(T(4,4)));

set(handles.T11,'string',' '); set(handles.T12,'string',' '); set(handles.T13,'string',' '); set(handles.T14,'string',' '); set(handles.T21,'string',' '); set(handles.T22,'string',' '); set(handles.T23,'string',' '); set(handles.T24,'string',' '); set(handles.T31,'string',' '); set(handles.T32,'string',' '); set(handles.T33,'string',' '); set(handles.T34,'string',' '); set(handles.T41,'string',' '); set(handles.T42,'string',' '); set(handles.T43,'string',' '); set(handles.T44,'string',' ');

7.2 Giao diện tính ma trận Jacoby

function varargout = Jacoby(varargin)

% JACOBY MATLAB code for Jacoby.fig

% JACOBY, by itself, creates a new JACOBY or raises the existing

% singleton*. %

% H = JACOBY returns the handle to a new JACOBY or the handle to

% the existing singleton*. %

% JACOBY('CALLBACK',hObject,eventData,handles,...) calls the

local

% function named CALLBACK in JACOBY.M with the given input arguments.

%

% JACOBY('Property','Value',...) creates a new JACOBY or raises

the

% existing singleton*. Starting from the left, property value pairs are

% applied to the GUI before Jacoby_OpeningFcn gets called. An

% unrecognized property name or invalid value makes property application

% stop. All inputs are passed to Jacoby_OpeningFcn via varargin.

%

% *See GUI Options on GUIDE's Tools menu. Choose "GUI allows only one

% instance to run (singleton)". %

% See also: GUIDE, GUIDATA, GUIHANDLES

% Edit the above text to modify the response to help Jacoby

% Last Modified by GUIDE v2.5 06-Jan-2022 22:36:35

% Begin initialization code - DO NOT EDIT

gui_Singleton = 1;

gui_State = struct('gui_Name', mfilename, ...

'gui_Singleton', gui_Singleton, ...

'gui_OpeningFcn', @Jacoby_OpeningFcn, ... 'gui_OutputFcn', @Jacoby_OutputFcn, ...

'gui_LayoutFcn', [] , ...

'gui_Callback' , []);

if nargin && ischar(varargin{1})

gui_State.gui_Callback = str2func(varargin{1});

if nargout

[varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});

else

gui_mainfcn(gui_State, varargin{:});

end

% End initialization code - DO NOT EDIT

% --- Executes just before Jacoby is made visible.

function Jacoby_OpeningFcn(hObject,

% This function has no % hObject

% eventdata % handles % varargin

% Choose default command line output for Jacoby

handles.output = hObject;

% Update handles structure

guidata(hObject, handles);

% UIWAIT makes Jacoby wait for user response (see UIRESUME)

% uiwait(handles.figure1);

% --- Outputs from this function are returned to the command line. function varargout = Jacoby_OutputFcn(hObject, eventdata, handles)

% varargout cell array for returning output args (see VARARGOUT);

% hObject

% eventdata % handles

% Get default command line output from handles structure

varargout{1} = handles.output;

function nhap_theta1_Callback(hObject, eventdata,

% hObject

% eventdata % handles

% Hints: get(hObject,'String') returns contents of nhap_theta1 as text

% str2double(get(hObject,'String')) returns contents of nhap_theta1 as a double theta1 = str2num(get(handles.nhap_theta1,'string')); if (theta1 > 180) theta1 = 180; end if (theta1 < -180) theta1 = -180; end set(handles.nhap_theta1,'string',num2str(theta1));

% --- Executes during object creation, after setting all properties. function nhap_theta1_CreateFcn(hObject, eventdata, handles)

% hObject

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

% hObject

% eventdata % handles

% Hints: get(hObject,'String') returns contents of nhap_theta6 as text

% str2double(get(hObject,'String')) returns contents of nhap_theta6 as a double theta6 = str2num(get(handles.nhap_theta6,'string')); if (theta6 > 360) theta6 = 360; end if (theta6 < -360) theta6 = -360; end set(handles.nhap_theta6,'string',num2str(theta6));

% --- Executes during object creation, after setting all properties. function nhap_theta6_CreateFcn(hObject, eventdata, handles)

% hObject

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

% hObject

% eventdata % handles

% Hints: get(hObject,'String') returns contents of nhap_theta5 as text

function nhap_theta5_CreateFcn(hObject,

% hObject

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

% hObject

% eventdata % handles

% Hints: get(hObject,'String') returns contents of nhap_theta4 as text

% str2double(get(hObject,'String')) returns contents of nhap_theta4 as a double theta4 = str2num(get(handles.nhap_theta4,'string')); if (theta4 > 170) theta4 = 170; end if (theta4 < -170) theta4 = -170; end set(handles.nhap_theta4,'string',num2str(theta4));

% --- Executes during object creation, after setting all properties. function nhap_theta4_CreateFcn(hObject, eventdata, handles)

% hObject

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

% hObject

% eventdata % handles

% Hints: get(hObject,'String') returns contents of nhap_theta2 as text

% str2double(get(hObject,'String')) returns contents of nhap_theta2 as a double theta2 = str2num(get(handles.nhap_theta2,'string')); if (theta2 > 90) theta2 = 90; end 49

if (theta2 < -85)

theta2 = -85;

end

set(handles.nhap_theta2,'string',num2str(theta2));

% --- Executes during object creation, after setting all properties. function nhap_theta2_CreateFcn(hObject, eventdata, handles)

% hObject

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

% hObject

% eventdata % handles

% Hints: get(hObject,'String') returns contents of nhap_theta3 as text

% str2double(get(hObject,'String')) returns contents of nhap_theta3 as a double theta3 = str2num(get(handles.nhap_theta3,'string')); if (theta3 > 260) theta3 = 260; end if (theta3 < -105) theta3 = -105; end set(handles.nhap_theta3,'string',num2str(theta3));

% --- Executes during object creation, after setting all properties. function nhap_theta3_CreateFcn(hObject, eventdata, handles)

% hObject

% eventdata % handles 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

% --- Executes on button press in Tinh.

function Tinh_Callback(hObject, eventdata, handles)

% hObject % eventdata % handles theta1 = str2num(get(handles.nhap_theta1,'string')); theta2 = str2num(get(handles.nhap_theta2,'string')); theta3 = str2num(get(handles.nhap_theta3,'string')); theta4 = str2num(get(handles.nhap_theta4,'string')); 50

theta5 = str2num(get(handles.nhap_theta5,'string')); theta6 = str2num(get(handles.nhap_theta6,'string')); %

%Tinh ma tran T06

a2 = 290; d4 = 270;

A1 = [cosd(theta1) 0 sind(theta1) 0;sind(theta1) 0 -cosd(theta1) 0;0100;0001];

A2 = [cosd(theta2) -sind(theta2) 0 a2*cosd(theta2);sind(theta2) cosd(theta2) 0 a2*sind(theta2);0 0 1 0;0 0 0 1];

A3 = [cosd(theta3) 0 sind(theta3) 0;sind(theta3) 0 -cosd(theta3) 0;0100;0001];

A4 = [cosd(theta4) 0 -sind(theta4) 0;sind(theta4) 0 cosd(theta4) 0;0 -1 0 d4;0 0 0 1];

A5 = [cosd(theta5) 0 sind(theta5) 0;sind(theta5) 0 -cosd(theta5) 0;0100;0001];

A6 = [cosd(theta6) -sind(theta6) 0 0;sind(theta6) cosd(theta6) 0 0;0010;0001]; % Tính Ti6 T56=A6; T46=A5*A6; T36=A4*A5*A6; T26=A6*A5*A3*A4; T16=A6*A2*A3*A4*A5; T06=A1*A2*A3*A4*A5*A6; %Tinh ma tran JH JH11 = T06(2,1)*T06(1,4)-T06(1,1)*T06(2,4); JH21 = T16(2,1)*T16(1,4)-T16(1,1)*T16(2,4); JH31 = T26(2,1)*T26(1,4)-T26(1,1)*T26(2,4); JH41 = T36(2,1)*T36(1,4)-T36(1,1)*T36(2,4); JH51 = T46(2,1)*T46(1,4)-T46(1,1)*T46(2,4); JH61 = T56(2,1)*T56(1,4)-T56(1,1)*T56(2,4); JH12 = T06(2,2)*T06(1,4)-T06(1,2)*T06(2,4); JH22 = T16(2,2)*T16(1,4)-T16(1,2)*T16(2,4); JH32 = T26(2,2)*T26(1,4)-T26(1,2)*T26(2,4); JH42 = T36(2,2)*T36(1,4)-T36(1,2)*T36(2,4); JH52 = T46(2,2)*T46(1,4)-T46(1,2)*T46(2,4); JH62 = T56(2,2)*T56(1,4)-T56(1,2)*T56(2,4); JH13 = T06(2,3)*T06(1,4)-T06(1,3)*T06(2,4); %c?t 3 JH23 = T16(2,3)*T16(1,4)-T16(1,3)*T16(2,4); JH33 = T26(2,3)*T26(1,4)-T26(1,3)*T26(2,4); JH43 = T36(2,3)*T36(1,4)-T36(1,3)*T36(2,4); JH53 = T46(2,3)*T46(1,4)-T46(1,3)*T46(2,4); JH63 = T56(2,3)*T56(1,4)-T56(1,3)*T56(2,4); JH14 = T06(3,1); %c?t 4 JH24 = T16(3,1); JH34 = T26(3,1); JH44 = T36(3,1); JH54 = T46(3,1); JH64 = T56(3,1);

JH56 = T46(3,3); JH66 = T56(3,3); Jh = [JH11,JH21,JH31,JH41,JH51,JH61; JH12,JH22,JH32,JH42,JH52,JH62; JH13,JH23,JH33,JH43,JH53,JH63; JH14,JH24,JH34,JH44,JH54,JH64; JH15,JH25,JH35,JH45,JH55,JH65; JH16,JH26,JH36,JH46,JH56,JH66]; R0n = T06(1:3,1:3); %ma tran R0n

%tinh ma tran Jacoby

J = [R0n,zeros(3,3);zeros(3,3),R0n]*Jh; %ghi data set(handles.J11,'string',num2str(J(1,1))); set(handles.J12,'string',num2str(J(1,2))); set(handles.J13,'string',num2str(J(1,3))); set(handles.J14,'string',num2str(J(1,4))); set(handles.J15,'string',num2str(J(1,5))); set(handles.J16,'string',num2str(J(1,6))); set(handles.J21,'string',num2str(J(2,1))); set(handles.J22,'string',num2str(J(2,2))); set(handles.J23,'string',num2str(J(2,3))); set(handles.J24,'string',num2str(J(2,4))); set(handles.J25,'string',num2str(J(2,5))); set(handles.J26,'string',num2str(J(2,6))); set(handles.J31,'string',num2str(J(3,1))); set(handles.J32,'string',num2str(J(3,2))); set(handles.J33,'string',num2str(J(3,3))); set(handles.J34,'string',num2str(J(3,4))); set(handles.J35,'string',num2str(J(3,5))); set(handles.J36,'string',num2str(J(3,6))); set(handles.J41,'string',num2str(J(4,1))); set(handles.J42,'string',num2str(J(4,2))); set(handles.J43,'string',num2str(J(4,3))); set(handles.J44,'string',num2str(J(4,4))); set(handles.J45,'string',num2str(J(4,5))); set(handles.J46,'string',num2str(J(4,6))); set(handles.J51,'string',num2str(J(5,1))); set(handles.J52,'string',num2str(J(5,2))); set(handles.J53,'string',num2str(J(5,3))); set(handles.J54,'string',num2str(J(5,4))); set(handles.J55,'string',num2str(J(5,5))); set(handles.J56,'string',num2str(J(5,6))); set(handles.J61,'string',num2str(J(6,1))); set(handles.J62,'string',num2str(J(6,2))); set(handles.J63,'string',num2str(J(6,3))); set(handles.J64,'string',num2str(J(6,4))); set(handles.J65,'string',num2str(J(6,5))); set(handles.J66,'string',num2str(J(6,6)));

% --- Executes on button press in Xoa.

function Xoa_Callback(hObject, eventdata, handles)

% hObject % eventdata % handles set(handles.nhap_theta1,'string',' '); set(handles.nhap_theta2,'string',' '); set(handles.nhap_theta3,'string',' '); set(handles.nhap_theta4,'string',' '); set(handles.nhap_theta5,'string',' '); set(handles.nhap_theta6,'string',' '); set(handles.J11,'string',' '); 52

set(handles.J12,'string',' '); set(handles.J13,'string',' '); set(handles.J14,'string',' '); set(handles.J15,'string',' '); set(handles.J16,'string',' '); set(handles.J21,'string',' '); set(handles.J22,'string',' '); set(handles.J23,'string',' '); set(handles.J24,'string',' '); set(handles.J25,'string',' '); set(handles.J26,'string',' '); set(handles.J31,'string',' '); set(handles.J32,'string',' '); set(handles.J33,'string',' '); set(handles.J34,'string',' '); set(handles.J35,'string',' '); set(handles.J36,'string',' '); set(handles.J41,'string',' '); set(handles.J42,'string',' '); set(handles.J43,'string',' '); set(handles.J44,'string',' '); set(handles.J45,'string',' '); set(handles.J46,'string',' '); set(handles.J51,'string',' '); set(handles.J52,'string',' '); set(handles.J53,'string',' '); set(handles.J54,'string',' '); set(handles.J55,'string',' '); set(handles.J56,'string',' '); set(handles.J61,'string',' '); set(handles.J62,'string',' '); set(handles.J63,'string',' '); set(handles.J64,'string',' '); set(handles.J65,'string',' '); set(handles.J66,'string',' ');

7.3 Thiết kế quỹ đạo

t = [0:0.005:2]; tAB = 2; %2s %diem A[100,20,-40,0,35,300]?B[130,-20,70,-40,100,-200] t1A = 100; t2A = 20; t3A = -40; t4A = 0; t5A = 35; t6A = 300; % khop 1 figure(1)

figure(2) q2_1 = t2A + 3*(t2B - t2A)/(tAB^2).*(t.^2) - 2*(t2B - t2A)/(tAB^3).*(t.^3); q2_2 = 2*(3*(t2B - t2A)/(tAB^2)).*t - 3*2*(t2B - t2A)/(tAB^3).*(t.^2); q2_3 = 2*(3*(t2B - t2A)/(tAB^2)) - 2*3*2*(t2B - t2A)/(tAB^3).*t; subplot(3,1,1)

plot(t,q2_1,'LineWidth',2);grid on;legend('Qu? ??o'); subplot(3,1,2) plot(t,q2_2,'LineWidth',2);grid on;legend('V?n t?c'); subplot(3,1,3) plot(t,q2_3,'LineWidth',2);grid on;legend('Gia t?c'); xlabel('time (s)'); %khop 3 figure(3) q3_1 = t3A + 3*(t3B - t3A)/(tAB^2).* (t.^2) - 2*(t3B - t3A)/(tAB^3).*(t.^3); q3_2 = 2*(3*(t3B - t3A)/(tAB^2)).*t - 3*2*(t3B - t3A)/(tAB^3).*(t.^2); q3_3 = 2*(3*(t3B - t3A)/(tAB^2)) - 2*3*2*(t3B - t3A)/(tAB^3).*t; subplot(3,1,1)

plot(t,q3_1,'LineWidth',2);grid on;legend('Qu? ??o'); subplot(3,1,2) plot(t,q3_2,'LineWidth',2);grid on;legend('V?n t?c'); subplot(3,1,3) plot(t,q3_3,'LineWidth',2);grid on;legend('Gia t?c'); xlabel('time (s)'); %khop 4 figure(4) q4_1 = t4A + 3*(t4B - t4A)/(tAB^2).*(t.^2) - 2*(t4B - t4A)/(tAB^3).*(t.^3); q4_2 = 2*(3*(t4B - t4A)/(tAB^2)).*t - 3*2*(t4B - t4A)/(tAB^3).*(t.^2); q4_3 = 2*(3*(t4B - t4A)/(tAB^2)) - 2*3*2*(t4B - t4A)/(tAB^3).*t; subplot(3,1,1)

plot(t,q4_1,'LineWidth',2);grid on;legend('Qu? ??o'); subplot(3,1,2) plot(t,q4_2,'LineWidth',2);grid on;legend('V?n t?c'); subplot(3,1,3) plot(t,q4_3,'LineWidth',2);grid on;legend('Gia t?c'); xlabel('time (s)'); %khop 5 figure(5) q5_1 = t5A + 3*(t5B - t5A)/(tAB^2).*(t.^2) - 2*(t5B - t5A)/(tAB^3).*(t.^3); q5_2 = 2*(3*(t5B - t5A)/(tAB^2)).*t - 3*2*(t5B - t5A)/(tAB^3).*(t.^2); q5_3 = 2*(3*(t5B - t5A)/(tAB^2)) - 2*3*2*(t5B - t5A)/(tAB^3).*t; subplot(3,1,1)

plot(t,q5_1,'LineWidth',2);grid on;legend('Qu? ??o'); subplot(3,1,2) plot(t,q5_2,'LineWidth',2);grid on;legend('V?n t?c'); subplot(3,1,3) plot(t,q5_3,'LineWidth',2);grid on;legend('Gia t?c'); xlabel('time (s)'); %khop 6 figure(6) q6_1 = t6A + 3*(t6B - t6A)/(tAB^2).*(t.^2) - 2*(t6B - t6A)/(tAB^3).*(t.^3); q6_2 = 2*(3*(t6B - t6A)/(tAB^2)).*t - 3*2*(t6B - t6A)/(tAB^3).*(t.^2); q6_3 = 2*(3*(t6B - t6A)/(tAB^2)) - 2*3*2*(t6B - t6A)/(tAB^3).*t; subplot(3,1,1)

plot(t,q6_1,'LineWidth',2);grid on;legend('Qu? ??o'); subplot(3,1,2)

plot(t,q6_2,'LineWidth',2);grid on;legend('V?n t?c');

subplot(3,1,3)

plot(t,q6_3,'LineWidth',2);grid on;legend('Gia t?c'); xlabel('time (s)');