% -------------------------------------------------------------------------- % chuong trinh lap phuong trinh dong hoc tay may theo quy tac DH % tac gia: khong minh % dia diem: 306 nha b3 truong dai hoc Phenikaa % thoi gian: 6h59' % -------------------------------------------------------------------------- % -------------------------------------------------------------------------- % cac thao tac voi man hinh clc; % Xoa sach man hinh clear all; % Xoa gia tri cua cac bien truoc do close all; % Dong tat ca cac chuong trinh dang chay truoc do %-------------------------------------------------------------------------- % khai bao bieu tuong cua cac gia tri bien khop va kich thuoc tay may syms d_i teta_i a_i anpha_i; % Ten cac tham so dong hoc cua bang DH syms q1 q2 q2 q3 q4 q6 q7 q8; % cac bien khop syms l1 l2 l3 l4 l5 l6 l7 l8 a1 a2 a3 b1 b2 b3; % kich thuoc cua cac khau %-------------------------------------------------------------------------- %----hien thi ma tran DH tong quat ra man hinh H_i_j=[cos(teta_i) -sin(teta_i)*cos(anpha_i) sin(teta_i)*sin(anpha_i) a_i*cos(teta_i); sin(teta_i) cos(teta_i)*cos(anpha_i) -cos(teta_i)*sin(anpha_i) a_i*sin(teta_i); 0 sin(anpha_i) cos(anpha_i) d_i; 0 0 0 1]; disp('----------------------------------------------------------------------------------------------') disp('---------------------------------MA TRAN D-H TONG QUAT----------------------------------------'); disp('----------------------------------------------------------------------------------------------') pretty(simplify(collect(H_i_j))) % cac lenh lam dep, viet gon lai, nhom cac gia tri trong ma tran disp('----------------------------------------------------------------------------------------------') %-------------------------------------------------------------------------- % nhap bang thong so dong hoc DH disp('moi cac ban nhap cac thong so cua bang dong hoc DH theo dong') disp('neu dong nao thua thi nhap tat ca cac gia tri bang 0') %-------------------------------------------------------------------------- %--------nhap dong thu nhat cua bang DH------------------------------------ disp('nhap dong thu nhat cua bang dong hoc DH') d1=input('d1=') teta1=input('teta1=') a1=input('a1=') anpha1=input('anpha1=') H_1_0=subs(H_i_j,[d_i,teta_i, a_i,anpha_i],[d1,teta1,a1,anpha1]); % thay cac gia tri vua nhap vao cong thuc DH % subs(ten ma tran goc,[bien ma tran goc],[gia tri tuong ung ma tran du lieu]) pretty(simplify(collect(H_1_0))) % cac lenh lam dep, viet gon lai, nhom cac gia tri trong ma tran %-------------------------------------------------------------------------- %--------nhap dong thu hai cua bang DH------------------------------------ disp('nhap dong thu hai cua bang dong hoc DH') d2=input('d2=') teta2=input('teta2=') a2=input('a2=') anpha2=input('anpha2=') H_2_1=subs(H_i_j,[d_i,teta_i, a_i,anpha_i],[d2,teta2,a2,anpha2]); % thay cac gia tri vua nhap vao cong thuc DH % subs(ten ma tran goc,[bien ma tran goc],[gia tri tuong ung ma tran du lieu]) pretty(simplify(collect(H_2_1))) % cac lenh lam dep, viet gon lai, nhom cac gia tri trong ma tran %-------------------------------------------------------------------------- disp('nhap dong thu ba cua bang dong hoc DH') d3=input('d3=') teta3=input('teta3=') a3=input('a3=') anpha3=input('anpha3=') H_3_2=subs(H_i_j,[d_i,teta_i, a_i,anpha_i],[d3,teta3,a3,anpha3]); % thay cac gia tri vua nhap vao cong thuc DH % subs(ten ma tran goc,[bien ma tran goc],[gia tri tuong ung ma tran du lieu]) pretty(simplify(collect(H_3_2))) % cac lenh lam dep, viet gon lai, nhom cac gia tri trong ma tran %-------------------------------------------------------------------------- disp('nhap dong thu tu cua bang dong hoc DH') d4=input('d4=') teta4=input('teta4=') a4=input('a4=') anpha4=input('anpha4=') H_4_3=subs(H_i_j,[d_i,teta_i, a_i,anpha_i],[d4,teta4,a4,anpha4]); % thay cac gia tri vua nhap vao cong thuc DH % subs(ten ma tran goc,[bien ma tran goc],[gia tri tuong ung ma tran du lieu]) pretty(simplify(collect(H_4_3))) % cac lenh lam dep, viet gon lai, nhom cac gia tri trong ma tran %-------------------------------------------------------------------------- disp('nhap dong thu nam cua bang dong hoc DH') d5=input('d5=') teta5=input('teta5=') a5=input('a5=') anpha5=input('anpha5=') H_5_4=subs(H_i_j,[d_i,teta_i, a_i,anpha_i],[d5,teta5,a5,anpha5]); % thay cac gia tri vua nhap vao cong thuc DH % subs(ten ma tran goc,[bien ma tran goc],[gia tri tuong ung ma tran du lieu]) pretty(simplify(collect(H_5_4))) % cac lenh lam dep, viet gon lai, nhom cac gia tri trong ma tran %-------------------------------------------------------------------------- disp('nhap dong thu sau cua bang dong hoc DH') d6=input('d6=') teta6=input('teta6=') a6=input('a6=') anpha6=input('anpha6=') H_6_5=subs(H_i_j,[d_i,teta_i, a_i,anpha_i],[d6,teta6,a6,anpha6]); % thay cac gia tri vua nhap vao cong thuc DH % subs(ten ma tran goc,[bien ma tran goc],[gia tri tuong ung ma tran du lieu]) pretty(simplify(collect(H_6_5))) % cac lenh lam dep, viet gon lai, nhom cac gia tri trong ma tran %-------------------------------------------------------------------------- %--------tinh ma tran D - phuong trinh dong hoc thuan---------------------- disp('ma tran D - phuong trinh dong hoc thuan') D = H_1_0*H_2_1*H_3_2*H_4_3*H_5_4*H_6_5; % pretty(simplify(collect(D))) pretty(D)
% Chuong trinh thiet ke quy dao bac 3 theo thoi gian % Khong Minh % 501 - A1 PU % 16h52' ngay 12/6/2024 %-------------------------------------------------------------------------- clc; clear all; close all; %-------------------------------------------------------------------------- % Khai bao cac dieu kien bien qi = 0; % (rad) goc tai thoi diem xuat phat wi = 0; % (rad/s) van toc tai thoi diem xuat phat qf = pi/3; % (rad) goc tai thoi diem ket thuc wf = 0; % (rad/s) van toc tai thoi diemket thuc %-------------------------------------------------------------------------- % khai bao thoi gian chuyen dong tu vi tri xuat phat den vi tri dich t = 2; % (s) %-------------------------------------------------------------------------- % xay dung ma tran Q Q = [qi; wi; qf; wf]; %-------------------------------------------------------------------------- % xay dung ma tran t T = [0,0,0,1; 0,0,1,0; t^3,t^2,t,1; 3*t^2,2*t,1,0]; %------------------------------------------------------------------------- % Tinh ma tran A A = T^-1*Q; a3 = A(1,1) a2 = A(2,1) a1 = A(3,1) a0 = A(4,1) %------------------------------------------------------------------------- % Ve do thi goc quay, van toc, gia toc cua khop q tt = linspace(0,t,100); % Tao mot mang thoi gian tu 0 den t chia deu 100 diem qt = a3*tt.^3 + a2*tt.^2+a1*tt+a0; wt = 3*a3*tt.^2+2*a2*tt+a1; et = 6*a3*tt+2*a2; %--------- plot(tt,qt) grid on hold on plot(tt,wt) plot(tt,et)
function D_n_0 = denavitHartenbergMatrices() % Nhap so luong ma tran DH tu ban phim n = input('Nhap so bac tu do cua robot: '); % Tao cell array de luu tru cac ma tran DH H_all = cell(n, 1); % Lap qua tung lien ket de nhap cac thong so DH va tinh ma tran bien doi for i = 1:n % Nhap cac thong so DH tu ban phim duoi dang ky hieu fprintf('Nhap cac gia tri cua bang DH %d: ', i); theta = sym(input('Nhap goc theta : ', 's')); d = sym(input('Nhap d : ', 's')); a = sym(input('Nhap a : ', 's')); alpha = sym(input('Nhap goc alpha : ', 's'));
% Tinh ma tran bien doi DH H = [cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha), a*cos(theta); sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha), a*sin(theta); 0, sin(alpha), cos(alpha), d; 0, 0, 0, 1];
% Luu ma tran vao cell array H_all{i} = H;
% Hien thi ma tran H voi dinh dang dep fprintf('Ma tran bien doi DH H_%d_%d la: ', i, i-1); disp(vpa(H, 4)); % hien thi ma tran H voi 4 chu so thap phan end % Tinh ma tran tong the D_n_0 = H_1_0 * H_2_1 * ... * H_n_(n-1) D_n_0 = H_all{1}; for i = 2:n D_n_0 = D_n_0 * H_all{i}; end
% Hien thi ma tran tong the D_n_0 fprintf('Ma tran D_%d_0 la: ', n); disp(vpa(D_n_0, 4)); % hien thi ma tran tong the voi 4 chu so thap phan end
function H = denavitHartenbergMatrix_syms() % Nhap cac thong so DH tu ban phim duoi dang ky hieu syms theta d a alpha
% Nhap cac ky hieu theta = sym(input('Nhap ky hieu goc theta (vi du: theta1): ', 's')); d = sym(input('Nhap ky hieu khoang cach d (vi du: d1): ', 's')); a = sym(input('Nhap ky hieu khoang cach a (vi du: a1): ', 's')); alpha = sym(input('Nhap ky hieu goc alpha (vi du: alpha1): ', 's'));
% Tinh ma tran bien doi DH H = [cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha), a*cos(theta); sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha), a*sin(theta); 0, sin(alpha), cos(alpha), d; 0, 0, 0, 1];
% Hien thi ma tran H voi dinh dang dep fprintf('Ma tran bien doi DH H la: '); disp(vpa(H, 4)); % hien thi ma tran H voi 4 chu so thap phan end
n = 800; A = 1.995653; B = 1.27689; C = 8; r=linspace(0,1,n); theta=linspace(-2,20*pi,n); [R,THETA]=ndgrid(r,theta); % define the number of petals we want per cycle. Roses have 3 and a bit. petalNum=3.6; x = 1 - (1/2)*((5/4)*(1 - mod(petalNum*THETA, 2*pi)/pi).^2 - 1/4).^2; phi = (pi/2)*exp(-THETA/(C*pi)); y = A*(R.^2).*(B*R - 1).^2.*sin(phi); R2 = x.*(R.*sin(phi) + y.*cos(phi)); X=R2.*sin(THETA); Y=R2.*cos(THETA); Z=x.*(R.*cos(phi)-y.*sin(phi)); % % define a red map for our rose colouring red_map=linspace(1,0.25,10)'; red_map(:,2)=0; red_map(:,3)=0; clf surf(X,Y,Z,'LineStyle','none') view([-40.50 42.00]) colormap(red_map)
%************************************************************************** % THONG TIN CHUONG TRINH % TEN CHUONG TRINH: CHUONG TRINH MO PHONG KHONG GIAN LAM VIEC CUA TAY MAY 3 % BAC TU DO RTR % TAC GIA: KHONG MINH % THOI GIAN: 16H21' % DIA DIEM: PHONG ROBOT TANG 1 B4 TRUONG DAI HOC PHENIKAA % THONG TIN CHUONG TRINH %************************************************************************** % THAO TAC BAN DAU clc; clear all; close all; % THAO TAC BAN DAU %************************************************************************** % xp = sin(q1)*(a2 - 1.0*q2) + a3*cos(q1)*cos(q3) - 1.0*a3*sin(q1)*sin(q3); % yp = a3*cos(q1)*sin(q3) - 1.0*cos(q1)*(a2 - 1.0*q2) + a3*cos(q3)*sin(q1); % zp = 0; %************************************************************************** % THONG TIN VE KICH THUOC TAY MAY VA KHONG GIAN KHOP q1 = linspace(0,pi,100);% Khong gian chuyen dong cua khop q1 q2 = linspace(0,1000,100); % Khong gian chuyen dong cua khop q2 q3 = linspace(0,2*pi/3,100); % Khong gian chuyen dong cua khop q3 a2 = 700; % (mm) a3 = 1000; % (mm) % THONG TIN VE KICH THUOC TAY MAY VA KHONG GIAN KHOP %************************************************************************** % TINH GIA TRI CUA DIEM TAC DONG CUOI P xp = sin(q1).*(a2 - q2) + a3*cos(q1).*cos(q3) - a3*sin(q1).*sin(q3); yp = a3*cos(q1).*sin(q3) - cos(q1).*(a2 - q2) + a3*cos(q3).*sin(q1); zp = linspace(0,0,100); % % TINH GIA TRI CUA DIEM TAC DONG CUOI P % %************************************************************************** % % VE DO THI CAC DIEM TAC DONG CUOI P plot3(xp,yp,zp); % % VE DO THI CAC DIEM TAC DONG CUOI P
![ร้อยพ่อพันแม่ - WanMai [Official MV]](http://i.ytimg.com/vi/XY2pNYASEbg/mqdefault.jpg)
![กี่หมื่นฟ้า | Your Sky Series EP.1 [1/4]](http://i.ytimg.com/vi/vLGjxFL6U_I/mqdefault.jpg)
cảm ơn thầy, thầy giảng rất dễ hiểu
@@vietucnguyen3482 cảm ơn em đã phản hồi
Cảm ơn kênh của thầy
@@firefighter9829 cảm ơn em, bao nhiêu năm làm giờ mới có một người xem cảm ơn
@ thầy giảng giống lối tư duy như thầy em giảng ở trường một cách toàn diện và sâu sắc . Rất may mắn khi em đọc bình luận và tìm được kênh của thầy.
Cảm ơn bạn!
cảm ơn a đã chia sẻ
% --------------------------------------------------------------------------
% chuong trinh lap phuong trinh dong hoc tay may theo quy tac DH
% tac gia: khong minh
% dia diem: 306 nha b3 truong dai hoc Phenikaa
% thoi gian: 6h59'
% --------------------------------------------------------------------------
% --------------------------------------------------------------------------
% cac thao tac voi man hinh
clc; % Xoa sach man hinh
clear all; % Xoa gia tri cua cac bien truoc do
close all; % Dong tat ca cac chuong trinh dang chay truoc do
%--------------------------------------------------------------------------
% khai bao bieu tuong cua cac gia tri bien khop va kich thuoc tay may
syms d_i teta_i a_i anpha_i; % Ten cac tham so dong hoc cua bang DH
syms q1 q2 q2 q3 q4 q6 q7 q8; % cac bien khop
syms l1 l2 l3 l4 l5 l6 l7 l8 a1 a2 a3 b1 b2 b3; % kich thuoc cua cac khau
%--------------------------------------------------------------------------
%----hien thi ma tran DH tong quat ra man hinh
H_i_j=[cos(teta_i) -sin(teta_i)*cos(anpha_i) sin(teta_i)*sin(anpha_i) a_i*cos(teta_i);
sin(teta_i) cos(teta_i)*cos(anpha_i) -cos(teta_i)*sin(anpha_i) a_i*sin(teta_i);
0 sin(anpha_i) cos(anpha_i) d_i;
0 0 0 1];
disp('----------------------------------------------------------------------------------------------')
disp('---------------------------------MA TRAN D-H TONG QUAT----------------------------------------');
disp('----------------------------------------------------------------------------------------------')
pretty(simplify(collect(H_i_j))) % cac lenh lam dep, viet gon lai, nhom cac gia tri trong ma tran
disp('----------------------------------------------------------------------------------------------')
%--------------------------------------------------------------------------
% nhap bang thong so dong hoc DH
disp('moi cac ban nhap cac thong so cua bang dong hoc DH theo dong')
disp('neu dong nao thua thi nhap tat ca cac gia tri bang 0')
%--------------------------------------------------------------------------
%--------nhap dong thu nhat cua bang DH------------------------------------
disp('nhap dong thu nhat cua bang dong hoc DH')
d1=input('d1=')
teta1=input('teta1=')
a1=input('a1=')
anpha1=input('anpha1=')
H_1_0=subs(H_i_j,[d_i,teta_i, a_i,anpha_i],[d1,teta1,a1,anpha1]); % thay cac gia tri vua nhap vao cong thuc DH
% subs(ten ma tran goc,[bien ma tran goc],[gia tri tuong ung ma tran du lieu])
pretty(simplify(collect(H_1_0))) % cac lenh lam dep, viet gon lai, nhom cac gia tri trong ma tran
%--------------------------------------------------------------------------
%--------nhap dong thu hai cua bang DH------------------------------------
disp('nhap dong thu hai cua bang dong hoc DH')
d2=input('d2=')
teta2=input('teta2=')
a2=input('a2=')
anpha2=input('anpha2=')
H_2_1=subs(H_i_j,[d_i,teta_i, a_i,anpha_i],[d2,teta2,a2,anpha2]); % thay cac gia tri vua nhap vao cong thuc DH
% subs(ten ma tran goc,[bien ma tran goc],[gia tri tuong ung ma tran du lieu])
pretty(simplify(collect(H_2_1))) % cac lenh lam dep, viet gon lai, nhom cac gia tri trong ma tran
%--------------------------------------------------------------------------
disp('nhap dong thu ba cua bang dong hoc DH')
d3=input('d3=')
teta3=input('teta3=')
a3=input('a3=')
anpha3=input('anpha3=')
H_3_2=subs(H_i_j,[d_i,teta_i, a_i,anpha_i],[d3,teta3,a3,anpha3]); % thay cac gia tri vua nhap vao cong thuc DH
% subs(ten ma tran goc,[bien ma tran goc],[gia tri tuong ung ma tran du lieu])
pretty(simplify(collect(H_3_2))) % cac lenh lam dep, viet gon lai, nhom cac gia tri trong ma tran
%--------------------------------------------------------------------------
disp('nhap dong thu tu cua bang dong hoc DH')
d4=input('d4=')
teta4=input('teta4=')
a4=input('a4=')
anpha4=input('anpha4=')
H_4_3=subs(H_i_j,[d_i,teta_i, a_i,anpha_i],[d4,teta4,a4,anpha4]); % thay cac gia tri vua nhap vao cong thuc DH
% subs(ten ma tran goc,[bien ma tran goc],[gia tri tuong ung ma tran du lieu])
pretty(simplify(collect(H_4_3))) % cac lenh lam dep, viet gon lai, nhom cac gia tri trong ma tran
%--------------------------------------------------------------------------
disp('nhap dong thu nam cua bang dong hoc DH')
d5=input('d5=')
teta5=input('teta5=')
a5=input('a5=')
anpha5=input('anpha5=')
H_5_4=subs(H_i_j,[d_i,teta_i, a_i,anpha_i],[d5,teta5,a5,anpha5]); % thay cac gia tri vua nhap vao cong thuc DH
% subs(ten ma tran goc,[bien ma tran goc],[gia tri tuong ung ma tran du lieu])
pretty(simplify(collect(H_5_4))) % cac lenh lam dep, viet gon lai, nhom cac gia tri trong ma tran
%--------------------------------------------------------------------------
disp('nhap dong thu sau cua bang dong hoc DH')
d6=input('d6=')
teta6=input('teta6=')
a6=input('a6=')
anpha6=input('anpha6=')
H_6_5=subs(H_i_j,[d_i,teta_i, a_i,anpha_i],[d6,teta6,a6,anpha6]); % thay cac gia tri vua nhap vao cong thuc DH
% subs(ten ma tran goc,[bien ma tran goc],[gia tri tuong ung ma tran du lieu])
pretty(simplify(collect(H_6_5))) % cac lenh lam dep, viet gon lai, nhom cac gia tri trong ma tran
%--------------------------------------------------------------------------
%--------tinh ma tran D - phuong trinh dong hoc thuan----------------------
disp('ma tran D - phuong trinh dong hoc thuan')
D = H_1_0*H_2_1*H_3_2*H_4_3*H_5_4*H_6_5;
% pretty(simplify(collect(D)))
pretty(D)
% Chuong trinh thiet ke quy dao bac 3 theo thoi gian
% Khong Minh
% 501 - A1 PU
% 16h52' ngay 12/6/2024
%--------------------------------------------------------------------------
clc;
clear all;
close all;
%--------------------------------------------------------------------------
% Khai bao cac dieu kien bien
qi = 0; % (rad) goc tai thoi diem xuat phat
wi = 0; % (rad/s) van toc tai thoi diem xuat phat
qf = pi/3; % (rad) goc tai thoi diem ket thuc
wf = 0; % (rad/s) van toc tai thoi diemket thuc
%--------------------------------------------------------------------------
% khai bao thoi gian chuyen dong tu vi tri xuat phat den vi tri dich
t = 2; % (s)
%--------------------------------------------------------------------------
% xay dung ma tran Q
Q = [qi;
wi;
qf;
wf];
%--------------------------------------------------------------------------
% xay dung ma tran t
T = [0,0,0,1;
0,0,1,0;
t^3,t^2,t,1;
3*t^2,2*t,1,0];
%-------------------------------------------------------------------------
% Tinh ma tran A
A = T^-1*Q;
a3 = A(1,1)
a2 = A(2,1)
a1 = A(3,1)
a0 = A(4,1)
%-------------------------------------------------------------------------
% Ve do thi goc quay, van toc, gia toc cua khop q
tt = linspace(0,t,100); % Tao mot mang thoi gian tu 0 den t chia deu 100 diem
qt = a3*tt.^3 + a2*tt.^2+a1*tt+a0;
wt = 3*a3*tt.^2+2*a2*tt+a1;
et = 6*a3*tt+2*a2;
%---------
plot(tt,qt)
grid on
hold on
plot(tt,wt)
plot(tt,et)
%-------------------------------------------------------------------------
clc;
clear all;
close all;
% Khai bao cac bien khop, tham so dong hoc
syms q1 q2 q3 l1 l2 l3 a1 a2 a3 b1 b2 b3 c1 c2 c3 d1 d2 d3 t
%Ket qua cua bai toan dong hoc thuan RRR
x = l2*cos(q1)*cos(q2) + l3*cos(q1)*cos(q2)*cos(q3) - 1.0*l3*cos(q1)*sin(q2)*sin(q3)
y = l2*cos(q2)*sin(q1) + l3*cos(q2)*cos(q3)*sin(q1) - 1.0*l3*sin(q1)*sin(q2)*sin(q3)
z = l1 + l2*sin(q2) + l3*cos(q2)*sin(q3) + l3*cos(q3)*sin(q2)
% Khai bao van toc cua khau tac dong cuoi
vx = 1;
vy = 2;
vz = 3;
%-------------------------------------------------------------V =[vx;
vy;
vz];
%Tinh cac dao ham rieng
dxdq1 = diff(x,q1)
dxdq2 = diff(x,q2)
dxdq3 = diff(x,q3)
%--------------------------------------
dydq1 = diff(y,q1)
dydq2 = diff(y,q2)
dydq3 = diff(y,q3)
%------------------------------------
dzdq1 = diff(z,q1)
dzdq2 = diff(z,q2)
dzdq3 = diff(z,q3)
%-----------------------------------------
% Tinh ma tran Jacobi tinh tien
J=simplify([dxdq1,dxdq2,dxdq3;
dydq1,dydq2,dydq3;
dzdq1,dzdq2,dzdq3;]);
%------------------------------------
% Tinh van toc goc cua cac khop
W = (J^-1)*V;
w1=W(1,1)
w2=W(2,1)
w3=W(3,1)
%--------------------------------------------------------------------------
% BAI TOAN DONG HOC NGUOC ROBOT CONG NGHIEP SCARA 3 BAC TU DO
%--------------------------------------------------------------------------
% Tac gia: Khong Minh
% Khoa Co khi - Co dien tu truong dai hoc Phenikaa
% Phong 306 nha B4
% Ngay 19/09/2023
%--------------------------------------------------------------------------
clc;
close all;
clear all;
%--------------------------------------------------------------------------
% THONG SO KICH THUOC CUA TAY MAY
%--------------------------------------------------------------------------
l1 = 500; %(mm)
l2 = 500; % (mm)
%--------------------------------------------------------------------------
% TOA DO DIEM TAC DONG CUOI
x = linspace(100,300,100); % (mm);
y = linspace(100,200,100); % (mm)
z = -150; % (mm)
%--------------------------------------------------------------------------
% TINH BAI TOAN DONG HOC NGUOC
%--------------------------------------------------------------------------
q2 = acos((x.*x+y.*y-l1*l1-l2*l2)/(2*l1*l2));
q1 = atan(y./x)- atan((l2*sin(q2))/(l1+l2*cos(q2)));
q3 = -z;
q2 = q2*180/pi;
q1 = q1*180/pi;
%--------------------------------------------------------------------------
disp('KET QUA CUA BAI TOAN DONG HOC NGUOC LA:')
disp('q1=');disp(q1);
disp('q2=');disp(q2);
disp('q3=');disp(q3);
%--------------------------------------------------------------------------
plot(q1,'-');
hold on;
plot(q2,'o');
grid on;
hold off;
% ---- chuong trinh tinh ma tran Jacobi cho tay may toa do cau RRR
%--------------------------------------------------------------------------
% tac gia: Khong Minh
% thoi gian: 12h10 ngay 4/11/2022
% dia diem: phong 306 b4 truong dai hoc Phenikaa
%--------------------------------------------------------------------------
clc;
close all;
clear all;
%--------------------------------------------------------------------------
% khai bao cac bien
syms q1 q2 q3 % cac bien khop
syms l1 l2 l3% kich thuoc tay may
syms vx vy vz % van toc theo cac truc x, y z
v(1,1) = vx;
v(2,1) = vy;
v(3,1) = vz;
%--------------------------------------------------------------------------
% ket qua bai toan dong hoc thuan
x = cos(q1)*(l2*cos(q2)+l2*cos(q2+q3));
y = sin(q1)*(l2*cos(q2)+l2*cos(q2+q3));
z = l1+l2*sin(q2)+l3*sin(q2+q3);
%--------------------------------------------------------------------------
% xay dung ma tran Jacobi tinh tien
% cot 1
Jt(1,1)=diff(x,q1);
Jt(2,1)=diff(y,q1);
Jt(3,1)=diff(z,q1);
% cot 2
Jt(1,2)=diff(x,q2);
Jt(2,2)=diff(y,q2);
Jt(3,2)=diff(z,q2);
% cot 3
Jt(1,3)=diff(x,q3);
Jt(2,3)=diff(y,q3);
Jt(3,3)=diff(z,q3);
%--------------------------------------------------------------------------
% % v = Jt*w
% syms t;
% q1 = 3*t+1;
% q2 = 2*sin(t)+pi/2;
% q3 = t-2;
% %--------------------------------------------------------------------------
% w1 = diff(q1,t);
% w(1,1)=w1;
% w2 = diff(q2,t);
% w(2,1)=w2;
% w3 = diff(q3,t);
% w(3,1)=w3;
% v=Jt*w
% %--------------------------------------------------------------------------
% % vx=l2^2*sin(q2 + l2*cos(q2 + q3))*sin(q2 + q3)*cos(q1) - 3*l2*cos(q2 + l2*cos(q2 + q3))*sin(q1) + 2*l2*sin(q2 + l2*cos(q2 + q3))*cos(q1)*cos(t)*(l2*sin(q2 + q3) - 1)
% % vy=3*l2*cos(q2 + l2*cos(q2 + q3))*cos(q1) + l2^2*sin(q2 + l2*cos(q2 + q3))*sin(q2 + q3)*sin(q1) + 2*l2*sin(q2 + l2*cos(q2 + q3))*cos(t)*sin(q1)*(l2*sin(q2 + q3) - 1)
% % vz= l3*cos(q2 + q3) + 2*cos(t)*(l3*cos(q2 + q3) + l2*cos(q2))
%
% %--------------------------------------------------------------------------
% t = linspace(0,10,1000);
% l1 =1;
% l2 =1;
% l3 =1;
% q1 = 3*cos(t)+1;
% q2 = 2*sin(t)+pi/2;
% q3 = t-2;
% % %--------------------------------------------------------------------------
% vx=l2^2*sin(q2 + l2*cos(q2 + q3)).*sin(q2 + q3).*cos(q1) - 3*l2*cos(q2 + l2*cos(q2 + q3)).*sin(q1) + 2*l2*sin(q2 + l2*cos(q2 + q3)).*cos(q1).*cos(t).*(l2*sin(q2 + q3) - 1)
% vy=3*l2*cos(q2 + l2*cos(q2 + q3)).*cos(q1) + l2^2*sin(q2 + l2*cos(q2 + q3)).*sin(q2 + q3).*sin(q1) + 2*l2*sin(q2 + l2*cos(q2 + q3)).*cos(t).*sin(q1).*(l2*sin(q2 + q3) - 1)
% %%
% %
% % PREFORMATTED
% % TEXT
% %
% vz=l3*cos(q2 + q3) + 2*cos(t).*(l3*cos(q2 + q3) + l2*cos(q2))
%
% % %--------------------------------------------------------------------------
% x = vx.*t;
% y = vy.*t;
% z= vz.*t
% plot3(x,y,z)
% pretty(simplify(Jt));
% pretty(simplify(Jt^-1));
w = (Jt^-1)*v;
%--------------------------
w1 = w(1,1);
w2 = w(2,1);
w3 = w(3,1);
%-----------------------------
% w1 = simplify(collect(w1));
% disp('w1=')
% pretty(w1)
%------------------------------
% w2 = simplify(collect(w2));
% disp('w2=')
% pretty(w2)
%--------------------------------
% w3 = simplify(collect(w3));
% disp('w3=')
% pretty(w3)
%---------------------------
% KHAO SAT CHUYEN DONG CUA CAC KHOP TRONG KHOANG THOI GIAN t
%--------------------------------------------------------------------------
t = linspace(0,5,500); % Tao mang thoi gian t = 5(s) chia thanh 500 khoang bang nhau
q1t = 3*t.^3+2*t.^2+t;
q2t = 0.5*t.^3+(1/3)*t.^2+0.25*t;
q3t = 2*t.^3+3*t.^2+1;
%--------------------------------------------------------------------------
vyt = 1;
vxt = 1;
vzt = 1;
l1t = 0.5;
l2t = 0.5;
l3t = 0.5;
w1 = (vyt*cos(q1t))./(l2t*cos(q2t + q3t).*(cos(q1t).^2)) + l2t*cos(q2t + q3t).*(sin(q1t).^2) + l2t*(cos(q1t).^2).*cos(q2t) + l2t*cos(q2t).*(sin(q1t).^2) - (vxt*sin(q1t))./((l2t*cos(q2t + q3t).*(cos(q1t).^2)) + l2t*cos(q2t + q3t).*(sin(q1t).^2) + l2t*(cos(q1t).^2).*cos(q2t) + l2t*cos(q2t).*(sin(q1t).^2))
w2 = (2*l2t*vzt*cos(q2t).*sin(q3t) + 2*l2t*vzt*cos(q3t).*sin(q2t) + 2*l3t*vxt*cos(q1t).*cos(q2t).*cos(q3t) + 2*l3t*vyt*cos(q2t).*cos(q3t).*sin(q1t) - 2*l3t*vxt*cos(q1t).*sin(q2t).*sin(q3t) - 2*l3t*vyt*sin(q1t).*sin(q2t).*sin(q3t))./(l2t*(l2t*sin(q3t) + l3t*sin(q3t) + l2t*sin(2*q2t + q3t) - l3t*sin(2*q2t + q3t)))
% plot(t,q1)
% grid on;
% hold on;
% plot(t,q2)
% plot(t,q3)
plot(t,w1)
grid on
hold on
plot (t,w2)
% plot(t,w3)
% thao tac voi man hinh, bien, chuong trinh
clc;
clear all;
close all;
%---------------------------
% khai bao cac tham so dong hoc tay may va vi tri cua khau tac dong cuoi
l1 = 1.0;
l2 = 1.0;
x_P = 1.23;
y_P = -1.1;
z_P = 0.7;
% uoc luong nghiem q1, q2, q3
initial_guess = [0.5, 0.5, 0.5];
% tao option trinh bay qua trinh tinh nghiem
options = optimoptions('fsolve', 'Display', 'iter');
% goi ham fsolve trong matlab
solution = fsolve(@(vars) mySystemSCARA(vars, l1, l2, x_P, y_P, z_P), initial_guess, options);
% hien thi ket qua
disp('nghiem cua he phuong trinh:');
disp(['q1 = ', num2str(solution(1))]);
disp(['q2 = ', num2str(solution(2))]);
disp(['q3 = ', num2str(solution(3))]);
% disp(['q4 = ', num2str(solution(4))]);
%-----------
% kiem tra ket qua
q1 = solution(1);
q2 = solution(2);
q3 = solution(3);
xp = l1*cos(q1) + l2*cos(q1+q2)
yp = l1*sin(q1) + l2*sin(q1+q2)
zp = -q3
function D_n_0 = denavitHartenbergMatrices()
% Nhap so luong ma tran DH tu ban phim
n = input('Nhap so bac tu do cua robot: ');
% Tao cell array de luu tru cac ma tran DH
H_all = cell(n, 1);
% Lap qua tung lien ket de nhap cac thong so DH va tinh ma tran bien doi
for i = 1:n
% Nhap cac thong so DH tu ban phim duoi dang ky hieu
fprintf('Nhap cac gia tri cua bang DH %d:
', i);
theta = sym(input('Nhap goc theta : ', 's'));
d = sym(input('Nhap d : ', 's'));
a = sym(input('Nhap a : ', 's'));
alpha = sym(input('Nhap goc alpha : ', 's'));
% Tinh ma tran bien doi DH
H = [cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha), a*cos(theta);
sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha), a*sin(theta);
0, sin(alpha), cos(alpha), d;
0, 0, 0, 1];
% Luu ma tran vao cell array
H_all{i} = H;
% Hien thi ma tran H voi dinh dang dep
fprintf('Ma tran bien doi DH H_%d_%d la:
', i, i-1);
disp(vpa(H, 4)); % hien thi ma tran H voi 4 chu so thap phan
end
% Tinh ma tran tong the D_n_0 = H_1_0 * H_2_1 * ... * H_n_(n-1)
D_n_0 = H_all{1};
for i = 2:n
D_n_0 = D_n_0 * H_all{i};
end
% Hien thi ma tran tong the D_n_0
fprintf('Ma tran D_%d_0 la:
', n);
disp(vpa(D_n_0, 4)); % hien thi ma tran tong the voi 4 chu so thap phan
end
%ve thu bien thien cua diem tac dong cuoi khi cac khop chuyen dong
clc;
clear all;
close all;
%------------------------------------------------------------
l1 =1;
l2 =2; % (m) kich thuoc cua khau 1 va khau 2
q3 = 0.5;
n= 100;
for i = 1 : n
q1 = linspace(-pi/2, pi/2, n);
q2 = linspace(pi/9, -pi/9, n);
q3 = linspace(0, q3, n);
x = l1*cos(q1) + l2*cos(q1).*cos(q2) - l2*sin(q1).*sin(q2);
y = l1*sin(q1) + l2*cos(q1).*sin(q2) + l2.*cos(q2).*sin(q1);
z = -q3;
plot3(x,y,z,'-o')
grid on
hold off
end
function H = denavitHartenbergMatrix_syms()
% Nhap cac thong so DH tu ban phim duoi dang ky hieu
syms theta d a alpha
% Nhap cac ky hieu
theta = sym(input('Nhap ky hieu goc theta (vi du: theta1): ', 's'));
d = sym(input('Nhap ky hieu khoang cach d (vi du: d1): ', 's'));
a = sym(input('Nhap ky hieu khoang cach a (vi du: a1): ', 's'));
alpha = sym(input('Nhap ky hieu goc alpha (vi du: alpha1): ', 's'));
% Tinh ma tran bien doi DH
H = [cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha), a*cos(theta);
sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha), a*sin(theta);
0, sin(alpha), cos(alpha), d;
0, 0, 0, 1];
% Hien thi ma tran H voi dinh dang dep
fprintf('Ma tran bien doi DH H la:
');
disp(vpa(H, 4)); % hien thi ma tran H voi 4 chu so thap phan
end
n = 800;
A = 1.995653;
B = 1.27689;
C = 8;
r=linspace(0,1,n);
theta=linspace(-2,20*pi,n);
[R,THETA]=ndgrid(r,theta);
% define the number of petals we want per cycle. Roses have 3 and a bit.
petalNum=3.6;
x = 1 - (1/2)*((5/4)*(1 - mod(petalNum*THETA, 2*pi)/pi).^2 - 1/4).^2;
phi = (pi/2)*exp(-THETA/(C*pi));
y = A*(R.^2).*(B*R - 1).^2.*sin(phi);
R2 = x.*(R.*sin(phi) + y.*cos(phi));
X=R2.*sin(THETA);
Y=R2.*cos(THETA);
Z=x.*(R.*cos(phi)-y.*sin(phi));
% % define a red map for our rose colouring
red_map=linspace(1,0.25,10)';
red_map(:,2)=0;
red_map(:,3)=0;
clf
surf(X,Y,Z,'LineStyle','none')
view([-40.50 42.00])
colormap(red_map)
%**************************************************************************
% THONG TIN CHUONG TRINH
% TEN CHUONG TRINH: CHUONG TRINH MO PHONG KHONG GIAN LAM VIEC CUA TAY MAY 3
% BAC TU DO RTR
% TAC GIA: KHONG MINH
% THOI GIAN: 16H21'
% DIA DIEM: PHONG ROBOT TANG 1 B4 TRUONG DAI HOC PHENIKAA
% THONG TIN CHUONG TRINH
%**************************************************************************
% THAO TAC BAN DAU
clc;
clear all;
close all;
% THAO TAC BAN DAU
%**************************************************************************
% xp = sin(q1)*(a2 - 1.0*q2) + a3*cos(q1)*cos(q3) - 1.0*a3*sin(q1)*sin(q3);
% yp = a3*cos(q1)*sin(q3) - 1.0*cos(q1)*(a2 - 1.0*q2) + a3*cos(q3)*sin(q1);
% zp = 0;
%**************************************************************************
% THONG TIN VE KICH THUOC TAY MAY VA KHONG GIAN KHOP
q1 = linspace(0,pi,100);% Khong gian chuyen dong cua khop q1
q2 = linspace(0,1000,100); % Khong gian chuyen dong cua khop q2
q3 = linspace(0,2*pi/3,100); % Khong gian chuyen dong cua khop q3
a2 = 700; % (mm)
a3 = 1000; % (mm)
% THONG TIN VE KICH THUOC TAY MAY VA KHONG GIAN KHOP
%**************************************************************************
% TINH GIA TRI CUA DIEM TAC DONG CUOI P
xp = sin(q1).*(a2 - q2) + a3*cos(q1).*cos(q3) - a3*sin(q1).*sin(q3);
yp = a3*cos(q1).*sin(q3) - cos(q1).*(a2 - q2) + a3*cos(q3).*sin(q1);
zp = linspace(0,0,100);
% % TINH GIA TRI CUA DIEM TAC DONG CUOI P
% %**************************************************************************
% % VE DO THI CAC DIEM TAC DONG CUOI P
plot3(xp,yp,zp);
% % VE DO THI CAC DIEM TAC DONG CUOI P