clear all

Published on May 2017 | Categories: Documents | Downloads: 34 | Comments: 0 | Views: 408
of 14
Download PDF   Embed   Report

Comments

Content

clear all;
close all;
load duongtron n xr yr;
XR(1)=xr(1);
YR(1)=yr(1);
% in ra cau 'Please wait' de nhan biet chuong trinh bat dau chay
disp('Please wait');
% thoi gian chu ky (sampling time)
dt=0.1;
% KHOI TAO BIEN BAN DAU
% thoi gian bat dau
t(1)=0;
% chieu dai khau
L1=0.5;
L2=0.5;
% hang so duong k
k1=1;
k2=1;
% toa do diem E ban dau trung voi diem R
xe(1)=xr(1)+0.005; ye(1)=yr(1)+0.005;
% van toc diem R ban dau
vrx(1)=0;
vry(1)=0;
% gia tri ban dau cua bien khop
C2=(xr(1)^2+yr(1)^2-L1^2-L2^2)/(2*L1*L2);
S2=sqrt(1-C2^2);
% goc theta 2 - cong thuc (1.2)
T2r(1)=atan2(S2,C2);
C1=((L1+L2*cos(T2r(1)))*xr(1)+L2*sin(T2r(1))*yr(1))/(xr(1)^2+yr(1)^2);
S1=((L1+L2*cos(T2r(1)))*yr(1)-L2*sin(T2r(1))*xr(1))/(xr(1)^2+yr(1)^2);
% goc theta 1 - cong thuc (1.3)
T1r(1)=atan2(S1,C1);
% gia tri van toc ban dau cua khop
dT1r(1)=0;
dT2r(1)=0;
%== Vong lap chuong trinh chinh
%======================================================
m1=1;
% Gia tri M2 dung
m2=4;
% gia tri M2 uoc luong ban dau
me2=4;
M2E(1)=me2;
%L1=0.25;
%L2=0.25;
g=9.81;
% gia tri ban dau cua bien khop
C2=(xe(1)^2+ye(1)^2-L1^2-L2^2)/(2*L1*L2);
S2=sqrt(1-C2^2);
% goc theta 2 - cong thuc (1.2)
T2(1)=atan2(S2,C2);
C1=((L1+L2*cos(T2(1)))*xe(1)+L2*sin(T2(1))*ye(1))/(xe(1)^2+ye(1)^2);
S1=((L1+L2*cos(T2(1)))*ye(1)-L2*sin(T2(1))*xe(1))/(xe(1)^2+ye(1)^2);
% goc theta 1 - cong thuc (1.3)
T1(1)=atan2(S1,C1);
%n=50;
%a1=1;b1=1;a2=1;b2=1;
%T1(1)=T1r(1);
%T2(1)=T2r(1);
%T1r(1)=T1(1);
%T2r(1)=T2(1);

e1(1)= T1r(1)-T1(1);
e2(1)= T2r(1)-T2(1);
%Tau1(1)=0;
%Tau2(1)=0;
dT1r(1)=0;
dT2r(1)=0;
dT1(1)=0;
dT2(1)=0;
ddT1r(1)=0;
ddT2r(1)=0;
ddT1(1)=0;
ddT2(1)=0;
k1=1;
k2=1;
k3=1;
k4=1;
ME11=me2*L2^2+2*me2*L1*L2*cos(T2(1))+(m1+me2)*L1^2;
ME12=me2*L2^2+me2*L1*L2*cos(T2(1));
ME21=me2*L1*L2*cos(T2(1))+me2*L2^2;
ME22=me2*L2^2;
ME=[ME11 ME12;ME21 ME22];
for i=2:n
%thoi gian t (giay)
t(i)=(i-1)*dt;
%van toc diem R tai thoi diem i
vrx(i)=(xr(i)-xr(i-1))/dt;
vry(i)=(yr(i)-yr(i-1))/dt;
% Van toc goc tham chieu cac khop tai thoi diem i - cong thuc (1.5)
dT1r(i)=(cos(T1(i-1)+T2(i-1))*vrx(i)+sin(T1(i-1)+T2(i1))*vry(i))/(L1*sin(T2(i-1)));
dT2r(i)=-((L1*cos(T1(i-1))+L2*cos(T1(i-1)+T2(i-1)))*vrx(i)+
(L1*sin(T1(i-1))+L2*sin(T1(i-1)+T2(i-1)))*vry(i))/(L1*L2*sin(T2(i-1)));
% Gia toc goc tham chieu cac khop tai thoi diem i - cong thuc

(1.5)

ddT1r(i)=(dT1r(i)-dT1r(i-1))/dt;
ddT2r(i)=(dT2r(i)-dT2r(i-1))/dt;
% Gia tri bien khop tham chieu tai thoi diem i
T1r(i)=T1r(i-1)+dT1r(i)* dt;
T2r(i)=T2r(i-1)+dT2r(i)*dt;
ar(i)=L1*cos(T1r(i))+L2*cos(T1r(i)+T2r(i));
br(i)=L1*sin(T1r(i))+L2*sin(T1r(i)+T2r(i));
% Dinh nghia sai so
e1(i)= T1r(i)-T1(i-1);
e2(i)= T2r(i)-T2(i-1);
e=[e1(i) e2(i)];
de1(i)=(e1(i)-e1(i-1))/dt;
de2(i)=(e2(i)-e2(i-1))/dt;
de=[de1(i) de2(i)];
dde1(i)=(de1(i)-de1(i-1))/dt;
dde2(i)=(de2(i)-de2(i-1))/dt;
dde=[dde1(i) dde2(i)];
ddT1e(i)=ddT1r(i)+k1*de1(i)+k2*e1(i);
ddT2e(i)=ddT2r(i)+k3*de2(i)+k4*e2(i);
%ddT1e(i)=ddT1r(i);

%ddT2e(i)=ddT2r(i);
%ddT1r(i)=ddT1(i)+k1*de1(i);
%ddT2r(i)=ddT2(i)+k2*de2(i);
%luat cap nhat
W1=(L2^2+2*L1*L2*cos(T2(i-1))+L1^2)*ddT1(i-1)+(L2^2+L1*L2*cos(T2(i1)))*ddT2(i-1)-L1*L2*sin(T2(i-1))*(dT2(i-1)^2+2*dT1(i-1)*dT2(i-1))
+g*(L1*cos(T1(i-1))+L2*cos(T1(i-1)+T2(i-1)));
W2=(L2^2+L1*L2*cos(T2(i-1)))*ddT1(i-1)+L2^2*ddT2(i1)+L1*L2*sin(T2(i-1))*dT1(i-1)^2+g*L2*cos(T1(i-1)+T2(i-1));
W=[W1 W2];
mmu=k1*ME*(dde'+k1*de'+k2*e');
mmu=mmu'*W'
%mmu=k1*ME*(dde'+k1*de'+k2*e')*W;
%mmu=k1*ME;
me2=me2+mmu*dt;
M2E(i)=me2;
%phuong trinh dong luc hoc du doan
%ME11=me2*L2^2+2*me2*L1*L2*cos(T2(i-1))+(m1+me2)*L1^2;
%ME12=me2*L2^2+me2*L1*L2*cos(T2(i-1));
%ME21=me2*L1*L2*cos(T2(i-1))+me2*L2^2;
%ME22=me2*L2^2;
%ME=[ME11 ME12;ME21 ME22];
DDT=[ddT1e(i);ddT2e(i)];
VE1=me2*g*L2*cos(T1(i-1)+T2(i-1))+(m1+me2)*L1*g*cos(T1(i-1))me2*L1*L2*dT1(i-1)^2*sin(T2(i-1))-2*me2*L1*L2*sin(T2(i-1))*dT1(i1)*dT2(i-1);
VE2=me2*L1*L2*dT1(i-1)^2*sin(T2(i-1))+me2*g*L2*cos(T1(i-1)+T2(i-1));
VE=[VE1;VE2];
TauE=ME*DDT+VE;
%phuong trinh dong luc hoc thuc
M11=m2*L2^2+2*m2*L1*L2*cos(T2(i-1))+(m1+m2)*L1^2;
M12=m2*L2^2+m2*L1*L2*cos(T2(i-1));
M21=m2*L1*L2*cos(T2(i-1))+m2*L2^2;
M22=m2*L2^2;
M=[M11 M12;M21 M22];
DDTe=[ddT1r(i);ddT2r(i)];
%% matran ko trong to khong chua ddT
V1=m2*g*L2*cos(T1(i-1)+T2(i-1))+(m1+m2)*L1*g*cos(T1(i-1))m2*L1*L2*dT1(i-1)^2*sin(T2(i-1))-2*m2*L1*L2*sin(T2(i-1))*dT1(i1)*dT2(i-1);
V2=m2*L1*L2*dT1(i-1)^2*sin(T2(i-1))+m2*g*L2*cos(T1(i-1)+T2(i-1));
V=[V1;V2];
Tau=M*DDTe+V;
DDT=inv(M)*(TauE-V);
%DDT=inv(M)*(V);
%Goc - van toc - gia toc thuc
ddT1(i)=DDT(1);
ddT2(i)=DDT(2);
%ddT1(i)=ddT1e(i)
%ddT2(i)=ddT2e(i)
dT1(i)=dT1(i-1)+ddT1(i)*dt;
dT2(i)=dT2(i-1)+ddT2(i)*dt;
T1(i)=T1(i-1)+dT1(i)*dt;
T2(i)=T2(i-1)+dT2(i)*dt;
% Vi tri diem E tai thoi diem i duoc dung hinh bang hinh hoc
%xe(i)=L1*cos(T1(i))+L2*cos(T1(i)+T2(i));
%ye(i)=L1*sin(T1(i))+L2*sin(T1(i)+T2(i));

end;
hold on;
% hien thi qui dao: duong xanh duong: qui dao tham chieu (diem R) duong xanh la cay: qui dao diem E
%plot(xr,yr,xe,ye);
xlabel('Chieu dai (m)');
ylabel('Chieu dai (m)');
axis equal;
plot(ar,br,xe,ye);
xlabel('Chieu dai (m)');
ylabel('Chieu dai (m)');
%figure ;plot(t,T1r*180/pi);
% hien thi goc khop 1: duong xanh duong: goc tham chieu - duong xanh la
cay: goc thuc
figure ;plot(t,T1r*180/pi,t,T1*180/pi);
% hien thi goc khop 2: duong xanh duong: goc tham chieu - duong xanh la
cay: goc thuc
figure ;plot(t,T2r*180/pi,t,T2*180/pi);
% hien thi gia tri M2 uoc luong
figure ;plot(t,M2E);
disp('End');

% lap trinh track theo qui dao dtron
clear all;
close all;
disp('Please wait');
% thoi gian chu ky (sampling time)
dt=0.1;
% so vong lap tuong ung voi 125 giay
n=1700;
% KHOI TAO BIEN BAN DAU
% thoi gian bat dau
t(1)=0;
%% cac gia tri cua model
m1=1; m2=1; m3=1.5; m4=1; m5=2; M=2; g=10
% khoi luong uoc luong
Me5=2,1
% toa do diem E ban dau
xe(1)=0.7524; ye(1)=0.7011; ze(1)=0.3915;
% toa do diem R ban dau
xr(1)=0.7524; yr(1)=0.7011; zr(1)=0.3915;
% cac gia tri K
%k1=0.5 ; k2=0.5; k3=0.1; k4=1;
k1=4.5 ; k2=1; k3=0.1; k4=2; % hoi tu nhanh. neu K1<4.5 thi duong bam
lai co chieu ha xuong, neu 4.5<k1<10 thi htu cang cham. k1>10 thi lai
vot lo vao trong.
%k1=5 ; k2=5; k3=0.5; k4=5;
%k1=1 ; k2=10; k3=1; k4=1;
% chieu dai khau
a1=0.15;
a2=0.25;
a3=0.35;
a4=0.25; % thay doi a3 = 0.35 thi ko bi giat manh
d5=0.05;
L=0.5; a5=0.5

% goc phi diem E ban dau
%Te(1)=pi/4;
Te(1)=atan2(xr(1),yr(1));
% goc phi diem R ban dau
Tr(1)=Te(1);
%Ter(1)=0;
Ter(1)=Te(1);
% gia tri ban dau cua bien khop,
%T1(1)=1.1031; T5(1)=-0.3177; T3(1)=1.6366; T2(1)= 0.8302; T4(1)=2.4668;
% gia tri van toc ban dau cua khop
dT1(1)=0; dT2(1)=0; dT3(1)=0; dT4(1)=0; dT5(1)=0;
% gia tri ban dau cua bien khop T1
S1=(ye(1)-L*sin(Te(1)))/(sqrt((ye(1)-L*sin(Te(1)))^2+(xe(1)L*cos(Te(1)))^2));
C1=((xe(1)-L*cos(Te(1)))/(sqrt(ye(1)-L*sin(Te(1)))^2+(xe(1)L*cos(Te(1)))^2));
T1(1)=atan2(S1,C1);
%gia tri ban dau cua bien khop T5
T5(1)=Te(1)-T1(1);
% gia tri ban dau cua bien khop T3
C3=((xe(1)*cos(T1(1))+ye(1)*sin(T1(1))-a4-a1-L*cos(T5(1)))^2+(ze(1)d5)^2-a3^2-a2^2)/(2*a2*a3);
S3=(sqrt(1-C3^2));
T3(1)=atan2(S3,C3);
% gia tri ban dau cua bien khop T2
C2=(a3*S3*(ze(1)-d5)+(a3*S3+a2)*(xe(1)*cos(T1(1))+ye(1)*sin(T1(1))-a4a1-L*cos(T5(1))))/((a3*cos(T3(1))+a2)^2+(a3*sin(T3(1)))^2)
S2=(sqrt(1-C2^2))
T2(1)=atan2(S2,C2)
% gia tri ban dau cua bien khop T4
T4(1)=-(T2(1)+T3(1));
% gia tri van toc ban dau cua khop
dT1(1)=0; dT2(1)=0; dT3(1)=0; dT4(1)=0; dT5(1)=0;
% ve tay may o vi tri ban dau
X1=[0 a1*cos(T1(1)) (a1+a2*cos(T2(1)))*cos(T1(1)) (a1+a2*cos(T2(1))
+a3*cos(T2(1)+T3(1)))*cos(T1(1)) (a1+a2*cos(T2(1))+a3*cos(T2(1)+T3(1))
+a4)*cos(T1(1)) (a1+a2*cos(T2(1))+a3*cos(T2(1)+T3(1))+a4)*cos(T1(1))
xe(1)];
Y1=[0 a1*sin(T1(1)) (a1+a2*cos(T2(1)))*sin(T1(1)) (a1+a2*cos(T2(1))
+a3*cos(T2(1)+T3(1)))*sin(T1(1)) (a1+a2*cos(T2(1))+a3*cos(T2(1)+T3(1))
+a4)*sin(T1(1)) (a1+a2*cos(T2(1))+a3*cos(T2(1)+T3(1))+a4)*sin(T1(1))
ye(1)];
Z1=[0 0
a2*sin(T2(1))
a2*sin(T2(1))
+a3*sin(T2(1)+T3(1))
a2*sin(T2(1))+a3*sin(T2(1)+T3(1))
a2*sin(T2(1))+a3*sin(T2(1)+T3(1))+d5
ze(1)];
%ve tay may o vi tri ban dau
plot3(X1,Y1,Z1,'--m'); xlabel('hanh do (m)'); ylabel('tung do (m)');
zlabel('cao do (m)')
%sai so ban dau
e1(1)=xr(1)-xe(1);
e2(1)=yr(1)-ye(1);
e3(1)=zr(1)-ze(1);
eTe(1)=Ter(1)-Te(1);
%% Phan uoc luong
% matran ME(1) co sai so

ME11=(1/2*a1^2*m1 + cos(T1(1))^2*Me5*a1^2 + cos(T1(1))^2*Me5*a4^2 +
cos(T1(1)+T5(1))^2*Me5*a5^2 + cos(T1(1))^2*cos(T2(1)+T3(1))^2*Me5 +
cos(T1(1))^2*cos(T4(1))^2*m4 + Me5*sin(T1(1))^2*a1^2 +
Me5*sin(T1(1))^2*a4^2 + Me5*sin(T1(1)+T5(1))^2*a5^2 +
cos(T1(1))^2*a1^2*m2 + cos(T1(1))^2*a1^2*m3 + cos(T1(1))^2*a1^2*m4 +
cos(T1(1))^2*a1^2*Me5 + cos(T1(1))^2*a4^2*Me5 + sin(T1(1))^2*a1^2*m2 +
sin(T1(1))^2*a1^2*m3 + sin(T1(1))^2*a1^2*m4 + sin(T1(1))^2*a1^2*Me5 +
sin(T1(1))^2*a4^2*Me5 + cos(T1(1))^2*cos(T2(1)+T3(1))^2*Me5 +
2*cos(T1(1))^2*Me5*a1*a4 + 2*cos(T1(1))^2*cos(T2(1)+T3(1))*a1*Me5 +
2*cos(T1(1))^2*cos(T4(1))*a1*m4 +
2*cos(T1(1))^2*cos(T2(1)+T3(1))*a4*Me5 + 2*Me5*sin(T1(1))^2*a1*a4 +
2*cos(T1(1))^2*a1*a4*Me5 + 2*sin(T1(1))^2*a1*a4*Me5 +
cos(T1(1))^2*cos(T2(1))^2*Me5*a2^2 + cos(T2(1))^2*Me5*sin(T1(1))^2*a2^2
+ cos(T2(1)+T3(1))^2*Me5*sin(T1(1))^2*a3^2 +
cos(T1(1))^2*cos(T2(1))^2*a2^2*m3 + cos(T1(1))^2*cos(T2(1))^2*a2^2*Me5
+ cos(T1(1))^2*cos(T3(1)+T4(1))^2*a2^2*m4 +
cos(T2(1))^2*sin(T1(1))^2*a2^2*m3 + cos(T2(1))^2*sin(T1(1))^2*a2^2*Me5
+ cos(T3(1)+T4(1))^2*sin(T1(1))^2*a2^2*m4 +
cos(T2(1)+T3(1))^2*sin(T1(1))^2*a3^2*Me5 +
cos(T4(1))^2*sin(T1(1))^2*a3^2*m4 +
2*cos(T1(1))^2*cos(T2(1)+T3(1))*Me5*a1 +
2*cos(T1(1))^2*cos(T2(1)+T3(1))*Me5*a4 +
2*Me5*sin(T1(1))*sin(T1(1)+T5(1))*a1*a5 +
2*Me5*sin(T1(1))*sin(T1(1)+T5(1))*a4*a5 +
2*cos(T1(1))^2*cos(T2(1))*cos(T2(1)+T3(1))*Me5*a2 +
2*cos(T1(1))^2*cos(T2(1))*Me5*a1*a2 +
2*cos(T1(1))^2*cos(T2(1))*Me5*a2*a4 +
2*cos(T1(1))^2*cos(T2(1))*cos(T2(1)+T3(1))*a2*Me5 +
2*cos(T1(1))^2*cos(T3(1)+T4(1))*cos(T4(1))*a2*m4 +
2*cos(T2(1))*Me5*sin(T1(1))^2*a1*a2 +
2*cos(T2(1)+T3(1))*Me5*sin(T1(1))^2*a1*a3 +
2*cos(T2(1))*Me5*sin(T1(1))^2*a2*a4 +
2*cos(T2(1)+T3(1))*Me5*sin(T1(1))^2*a3*a4 +
2*cos(T1(1))^2*cos(T2(1))*a1*a2*m3 +
2*cos(T1(1))^2*cos(T2(1))*a1*a2*Me5 +
2*cos(T1(1))^2*cos(T3(1)+T4(1))*a1*a2*m4 +
2*cos(T1(1))^2*cos(T2(1))*a2*a4*Me5 +
2*cos(T2(1))*sin(T1(1))^2*a1*a2*m3 +
2*cos(T2(1))*sin(T1(1))^2*a1*a2*Me5 +
2*cos(T3(1)+T4(1))*sin(T1(1))^2*a1*a2*m4 +
2*cos(T2(1)+T3(1))*sin(T1(1))^2*a1*a3*Me5 +
2*cos(T4(1))*sin(T1(1))^2*a1*a3*m4 +
2*cos(T2(1))*sin(T1(1))^2*a2*a4*Me5 +
2*cos(T2(1)+T3(1))*sin(T1(1))^2*a3*a4*Me5 +
2*cos(T1(1))*cos(T1(1)+T5(1))*cos(T2(1)+T3(1))*Me5*a5 +
2*cos(T1(1))*cos(T1(1)+T5(1))*Me5*a1*a5 +
2*cos(T1(1))*cos(T1(1)+T5(1))*Me5*a4*a5 +
2*cos(T1(1))*cos(T1(1)+T5(1))*cos(T2(1))*Me5*a2*a5 +
2*cos(T2(1))*Me5*sin(T1(1))*sin(T1(1)+T5(1))*a2*a5 +
2*cos(T2(1)+T3(1))*Me5*sin(T1(1))*sin(T1(1)+T5(1))*a3*a5 +
2*cos(T2(1))*cos(T2(1)+T3(1))*Me5*sin(T1(1))^2*a2*a3 +
2*cos(T2(1))*cos(T2(1)+T3(1))*sin(T1(1))^2*a2*a3*Me5 +
2*cos(T3(1)+T4(1))*cos(T4(1))*sin(T1(1))^2*a2*a3*m4);
ME12=(cos(T1(1))*Me5*sin(T1(1))*sin(T2(1)+T3(1))*a1*a3 cos(T1(1))*sin(T1(1))*sin(T2(1)+T3(1))*a4*Me5 cos(T1(1))*cos(T2(1)+T3(1))*Me5*sin(T1(1))*sin(T2(1)+T3(1)) -

cos(T1(1))*Me5*sin(T1(1))*sin(T2(1)+T3(1))*a1 cos(T1(1))*Me5*sin(T1(1))*sin(T2(1)+T3(1))*a4 cos(T1(1)+T5(1))*Me5*sin(T1(1))*sin(T2(1)+T3(1))*a5 cos(T1(1))*cos(T2(1)+T3(1))*sin(T1(1))*sin(T2(1)+T3(1))*Me5 cos(T1(1))*cos(T2(1))*Me5*sin(T1(1))*sin(T2(1)+T3(1))*a2 cos(T1(1))*cos(T2(1)+T3(1))*Me5*sin(T1(1))*sin(T2(1))*a2 cos(T1(1))*sin(T1(1))*sin(T2(1)+T3(1))*a1*Me5 +
cos(T1(1))*Me5*sin(T1(1))*sin(T2(1)+T3(1))*a3*a4 +
cos(T1(1))*Me5*sin(T1(1)+T5(1))*sin(T2(1))*a2*a5 cos(T1(1)+T5(1))*Me5*sin(T1(1))*sin(T2(1))*a2*a5 +
cos(T1(1))*Me5*sin(T1(1)+T5(1))*sin(T2(1)+T3(1))*a3*a5 cos(T1(1))*cos(T2(1))*sin(T1(1))*sin(T2(1)+T3(1))*a2*Me5 cos(T1(1))*cos(T2(1)+T3(1))*sin(T1(1))*sin(T2(1))*a2*Me5 +
cos(T1(1))*sin(T1(1))*sin(T2(1)+T3(1))*a1*a3*Me5 +
cos(T1(1))*sin(T1(1))*sin(T2(1)+T3(1))*a3*a4*Me5 +
cos(T1(1))*cos(T2(1)+T3(1))*Me5*sin(T1(1))*sin(T2(1)+T3(1))*a3^2 +
cos(T1(1))*cos(T2(1)+T3(1))*sin(T1(1))*sin(T2(1)+T3(1))*a3^2*Me5 +
cos(T1(1))*cos(T2(1))*Me5*sin(T1(1))*sin(T2(1)+T3(1))*a2*a3 +
cos(T1(1))*cos(T2(1)+T3(1))*Me5*sin(T1(1))*sin(T2(1))*a2*a3 +
cos(T1(1))*cos(T2(1))*sin(T1(1))*sin(T2(1)+T3(1))*a2*a3*Me5 +
cos(T1(1))*cos(T2(1)+T3(1))*sin(T1(1))*sin(T2(1))*a2*a3*Me5);
ME13=(cos(T1(1))*Me5*sin(T1(1))*sin(T2(1)+T3(1))*a1*a3 cos(T1(1))*sin(T1(1))*sin(T2(1)+T3(1))*a4*Me5 cos(T1(1))*cos(T2(1)+T3(1))*Me5*sin(T1(1))*sin(T2(1)+T3(1)) cos(T1(1))*Me5*sin(T1(1))*sin(T2(1)+T3(1))*a1 cos(T1(1))*Me5*sin(T1(1))*sin(T2(1)+T3(1))*a4 cos(T1(1)+T5(1))*Me5*sin(T1(1))*sin(T2(1)+T3(1))*a5 cos(T1(1))*cos(T2(1)+T3(1))*sin(T1(1))*sin(T2(1)+T3(1))*Me5 cos(T1(1))*cos(T2(1))*Me5*sin(T1(1))*sin(T2(1)+T3(1))*a2 cos(T1(1))*sin(T1(1))*sin(T2(1)+T3(1))*a1*Me5 +
cos(T1(1))*Me5*sin(T1(1))*sin(T2(1)+T3(1))*a3*a4 +
cos(T1(1))*Me5*sin(T1(1)+T5(1))*sin(T2(1)+T3(1))*a3*a5 cos(T1(1))*cos(T2(1))*sin(T1(1))*sin(T2(1)+T3(1))*a2*Me5 cos(T1(1))*cos(T4(1))*sin(T1(1))*sin(T3(1)+T4(1))*a2*m4 +
cos(T1(1))*sin(T1(1))*sin(T2(1)+T3(1))*a1*a3*Me5 +
cos(T1(1))*sin(T1(1))*sin(T2(1)+T3(1))*a3*a4*Me5 +
cos(T1(1))*cos(T2(1)+T3(1))*Me5*sin(T1(1))*sin(T2(1)+T3(1))*a3^2 +
cos(T1(1))*cos(T2(1)+T3(1))*sin(T1(1))*sin(T2(1)+T3(1))*a3^2*Me5 +
cos(T1(1))*cos(T2(1))*Me5*sin(T1(1))*sin(T2(1)+T3(1))*a2*a3 +
cos(T1(1))*cos(T2(1))*sin(T1(1))*sin(T2(1)+T3(1))*a2*a3*Me5 +
cos(T1(1))*cos(T4(1))*sin(T1(1))*sin(T3(1)+T4(1))*a2*a3*m4);
ME14=(cos(T1(1))*sin(T1(1))*sin(T4(1))*a1*a3*m4 cos(T1(1))*cos(T4(1))*sin(T1(1))*sin(T4(1))*m4 cos(T1(1))*cos(T3(1)+T4(1))*sin(T1(1))*sin(T4(1))*a2*m4 cos(T1(1))*cos(T4(1))*sin(T1(1))*sin(T3(1)+T4(1))*a2*m4 cos(T1(1))*sin(T1(1))*sin(T4(1))*a1*m4 +
cos(T1(1))*cos(T4(1))*sin(T1(1))*sin(T4(1))*a3^2*m4 +
cos(T1(1))*cos(T3(1)+T4(1))*sin(T1(1))*sin(T4(1))*a2*a3*m4 +
cos(T1(1))*cos(T4(1))*sin(T1(1))*sin(T3(1)+T4(1))*a2*a3*m4);
ME15=(cos(T1(1)+T5(1))^2*Me5*a5^2 + Me5*sin(T1(1)+T5(1))^2*a5^2 +
Me5*sin(T1(1))*sin(T1(1)+T5(1))*a1*a5 +
Me5*sin(T1(1))*sin(T1(1)+T5(1))*a4*a5 +
cos(T1(1))*cos(T1(1)+T5(1))*cos(T2(1)+T3(1))*Me5*a5 +
cos(T1(1))*cos(T1(1)+T5(1))*Me5*a1*a5 +

cos(T1(1))*cos(T1(1)+T5(1))*Me5*a4*a5 +
cos(T1(1))*cos(T1(1)+T5(1))*cos(T2(1))*Me5*a2*a5 +
cos(T2(1))*Me5*sin(T1(1))*sin(T1(1)+T5(1))*a2*a5 +
cos(T2(1)+T3(1))*Me5*sin(T1(1))*sin(T1(1)+T5(1))*a3*a5);

ME21=(cos(T1(1))*Me5*sin(T1(1))*sin(T2(1)+T3(1))*a1*a3 cos(T1(1))*sin(T1(1))*sin(T2(1)+T3(1))*a4*Me5 cos(T1(1))*cos(T2(1)+T3(1))*Me5*sin(T1(1))*sin(T2(1)+T3(1)) cos(T1(1))*Me5*sin(T1(1))*sin(T2(1)+T3(1))*a1 cos(T1(1))*Me5*sin(T1(1))*sin(T2(1)+T3(1))*a4 cos(T1(1)+T5(1))*Me5*sin(T1(1))*sin(T2(1)+T3(1))*a5 cos(T1(1))*cos(T2(1)+T3(1))*sin(T1(1))*sin(T2(1)+T3(1))*Me5 cos(T1(1))*cos(T2(1))*Me5*sin(T1(1))*sin(T2(1)+T3(1))*a2 cos(T1(1))*cos(T2(1)+T3(1))*Me5*sin(T1(1))*sin(T2(1))*a2 cos(T1(1))*sin(T1(1))*sin(T2(1)+T3(1))*a1*Me5 +
cos(T1(1))*Me5*sin(T1(1))*sin(T2(1)+T3(1))*a3*a4 +
cos(T1(1))*Me5*sin(T1(1)+T5(1))*sin(T2(1))*a2*a5 cos(T1(1)+T5(1))*Me5*sin(T1(1))*sin(T2(1))*a2*a5 +
cos(T1(1))*Me5*sin(T1(1)+T5(1))*sin(T2(1)+T3(1))*a3*a5 cos(T1(1))*cos(T2(1))*sin(T1(1))*sin(T2(1)+T3(1))*a2*Me5 cos(T1(1))*cos(T2(1)+T3(1))*sin(T1(1))*sin(T2(1))*a2*Me5 +
cos(T1(1))*sin(T1(1))*sin(T2(1)+T3(1))*a1*a3*Me5 +
cos(T1(1))*sin(T1(1))*sin(T2(1)+T3(1))*a3*a4*Me5 +
cos(T1(1))*cos(T2(1)+T3(1))*Me5*sin(T1(1))*sin(T2(1)+T3(1))*a3^2 +
cos(T1(1))*cos(T2(1)+T3(1))*sin(T1(1))*sin(T2(1)+T3(1))*a3^2*Me5 +
cos(T1(1))*cos(T2(1))*Me5*sin(T1(1))*sin(T2(1)+T3(1))*a2*a3 +
cos(T1(1))*cos(T2(1)+T3(1))*Me5*sin(T1(1))*sin(T2(1))*a2*a3 +
cos(T1(1))*cos(T2(1))*sin(T1(1))*sin(T2(1)+T3(1))*a2*a3*Me5 +
cos(T1(1))*cos(T2(1)+T3(1))*sin(T1(1))*sin(T2(1))*a2*a3*Me5);
ME22=(cos(T2(1))^2*Me5*a2^2 + cos(T2(1)+T3(1))^2*Me5*a3^2 +
Me5*sin(T1(1))^2*sin(T2(1)+T3(1))^2 + cos(T2(1))^2*a2^2*m3 +
cos(T2(1))^2*a2^2*m4 + cos(T2(1))^2*a2^2*Me5 +
cos(T2(1)+T3(1))^2*a3^2*Me5 + sin(T1(1))^2*sin(T2(1)+T3(1))^2*Me5 +
cos(T1(1))^2*Me5*sin(T2(1))^2*a2^2 +
cos(T1(1))^2*Me5*sin(T2(1)+T3(1))^2*a3^2 +
Me5*sin(T1(1))^2*sin(T2(1))^2*a2^2 + cos(T1(1))^2*sin(T2(1))^2*a2^2*m3
+ cos(T1(1))^2*sin(T2(1))^2*a2^2*Me5 +
cos(T1(1))^2*sin(T2(1)+T3(1))^2*a3^2*Me5 +
sin(T1(1))^2*sin(T2(1))^2*a2^2*m3 + sin(T1(1))^2*sin(T2(1))^2*a2^2*Me5
+ 2*cos(T2(1))*cos(T2(1)+T3(1))*a2*a3*Me5 +
2*Me5*sin(T1(1))^2*sin(T2(1))*sin(T2(1)+T3(1))*a2 +
2*sin(T1(1))^2*sin(T2(1))*sin(T2(1)+T3(1))*a2*Me5 +
2*cos(T2(1))*cos(T2(1)+T3(1))*Me5*a2*a3 +
2*cos(T1(1))^2*Me5*sin(T2(1))*sin(T2(1)+T3(1))*a2*a3 +
2*cos(T1(1))^2*sin(T2(1))*sin(T2(1)+T3(1))*a2*a3*Me5);
ME23=(cos(T2(1)+T3(1))^2*Me5*a3^2 + Me5*sin(T1(1))^2*sin(T2(1)+T3(1))^2
+ cos(T2(1)+T3(1))^2*a3^2*Me5 + sin(T1(1))^2*sin(T2(1)+T3(1))^2*Me5 +
cos(T1(1))^2*Me5*sin(T2(1)+T3(1))^2*a3^2 +
cos(T1(1))^2*sin(T2(1)+T3(1))^2*a3^2*Me5 +
cos(T2(1))*cos(T2(1)+T3(1))*a2*a3*Me5 +
Me5*sin(T1(1))^2*sin(T2(1))*sin(T2(1)+T3(1))*a2 +
sin(T1(1))^2*sin(T2(1))*sin(T2(1)+T3(1))*a2*Me5 +

cos(T2(1))*cos(T2(1)+T3(1))*Me5*a2*a3 +
cos(T1(1))^2*Me5*sin(T2(1))*sin(T2(1)+T3(1))*a2*a3 +
cos(T1(1))^2*sin(T2(1))*sin(T2(1)+T3(1))*a2*a3*Me5);
ME24=(-cos(T2(1))*cos(T4(1))*a2*a3*m4);
ME25=(cos(T1(1))*Me5*sin(T1(1)+T5(1))*sin(T2(1))*a2*a5 cos(T1(1)+T5(1))*Me5*sin(T1(1))*sin(T2(1)+T3(1))*a5 cos(T1(1)+T5(1))*Me5*sin(T1(1))*sin(T2(1))*a2*a5 +
cos(T1(1))*Me5*sin(T1(1)+T5(1))*sin(T2(1)+T3(1))*a3*a5);

ME31=(cos(T1(1))*Me5*sin(T1(1))*sin(T2(1)+T3(1))*a1*a3 cos(T1(1))*sin(T1(1))*sin(T2(1)+T3(1))*a4*Me5 cos(T1(1))*cos(T2(1)+T3(1))*Me5*sin(T1(1))*sin(T2(1)+T3(1)) cos(T1(1))*Me5*sin(T1(1))*sin(T2(1)+T3(1))*a1 cos(T1(1))*Me5*sin(T1(1))*sin(T2(1)+T3(1))*a4 cos(T1(1)+T5(1))*Me5*sin(T1(1))*sin(T2(1)+T3(1))*a5 cos(T1(1))*cos(T2(1)+T3(1))*sin(T1(1))*sin(T2(1)+T3(1))*Me5 cos(T1(1))*cos(T2(1))*Me5*sin(T1(1))*sin(T2(1)+T3(1))*a2 cos(T1(1))*sin(T1(1))*sin(T2(1)+T3(1))*a1*Me5 +
cos(T1(1))*Me5*sin(T1(1))*sin(T2(1)+T3(1))*a3*a4 +
cos(T1(1))*Me5*sin(T1(1)+T5(1))*sin(T2(1)+T3(1))*a3*a5 cos(T1(1))*cos(T2(1))*sin(T1(1))*sin(T2(1)+T3(1))*a2*Me5 cos(T1(1))*cos(T4(1))*sin(T1(1))*sin(T3(1)+T4(1))*a2*m4 +
cos(T1(1))*sin(T1(1))*sin(T2(1)+T3(1))*a1*a3*Me5 +
cos(T1(1))*sin(T1(1))*sin(T2(1)+T3(1))*a3*a4*Me5 +
cos(T1(1))*cos(T2(1)+T3(1))*Me5*sin(T1(1))*sin(T2(1)+T3(1))*a3^2 +
cos(T1(1))*cos(T2(1)+T3(1))*sin(T1(1))*sin(T2(1)+T3(1))*a3^2*Me5 +
cos(T1(1))*cos(T2(1))*Me5*sin(T1(1))*sin(T2(1)+T3(1))*a2*a3 +
cos(T1(1))*cos(T2(1))*sin(T1(1))*sin(T2(1)+T3(1))*a2*a3*Me5 +
cos(T1(1))*cos(T4(1))*sin(T1(1))*sin(T3(1)+T4(1))*a2*a3*m4);
ME32=(cos(T2(1)+T3(1))^2*Me5*a3^2 + Me5*sin(T1(1))^2*sin(T2(1)+T3(1))^2
+ cos(T2(1)+T3(1))^2*a3^2*Me5 + sin(T1(1))^2*sin(T2(1)+T3(1))^2*Me5 +
cos(T1(1))^2*Me5*sin(T2(1)+T3(1))^2*a3^2 +
cos(T1(1))^2*sin(T2(1)+T3(1))^2*a3^2*Me5 +
cos(T2(1))*cos(T2(1)+T3(1))*a2*a3*Me5 +
Me5*sin(T1(1))^2*sin(T2(1))*sin(T2(1)+T3(1))*a2 +
sin(T1(1))^2*sin(T2(1))*sin(T2(1)+T3(1))*a2*Me5 +
cos(T2(1))*cos(T2(1)+T3(1))*Me5*a2*a3 +
cos(T1(1))^2*Me5*sin(T2(1))*sin(T2(1)+T3(1))*a2*a3);
ME33=(cos(T2(1)+T3(1))^2*Me5*a3^2 + Me5*sin(T1(1))^2*sin(T2(1)+T3(1))^2
+ cos(T2(1)+T3(1))^2*a3^2*Me5 + sin(T1(1))^2*sin(T2(1)+T3(1))^2*Me5 +
cos(T1(1))^2*Me5*sin(T2(1)+T3(1))^2*a3^2 +
cos(T1(1))^2*sin(T3(1)+T4(1))^2*a2^2*m4 +
cos(T1(1))^2*sin(T2(1)+T3(1))^2*a3^2*Me5 +
sin(T1(1))^2*sin(T3(1)+T4(1))^2*a2^2*m4);
ME34=(m4*cos(T1(1))^2*sin(T3(1)+T4(1))^2*a2^2 +
sin(T4(1))*a3*m4*cos(T1(1))^2*sin(T3(1)+T4(1))*a2 +
m4*sin(T1(1))^2*sin(T3(1)+T4(1))^2*a2^2 +
sin(T4(1))*m4*sin(T1(1))^2*sin(T3(1)+T4(1))*a2);

ME35=(cos(T1(1))*Me5*sin(T1(1)+T5(1))*sin(T2(1)+T3(1))*a3*a5 cos(T1(1)+T5(1))*Me5*sin(T1(1))*sin(T2(1)+T3(1))*a5);

ME41=(cos(T1(1))*sin(T1(1))*sin(T4(1))*a1*a3*m4 cos(T1(1))*cos(T4(1))*sin(T1(1))*sin(T4(1))*m4 cos(T1(1))*cos(T3(1)+T4(1))*sin(T1(1))*sin(T4(1))*a2*m4 cos(T1(1))*cos(T4(1))*sin(T1(1))*sin(T3(1)+T4(1))*a2*m4 cos(T1(1))*sin(T1(1))*sin(T4(1))*a1*m4 +
cos(T1(1))*cos(T4(1))*sin(T1(1))*sin(T4(1))*a3^2*m4 +
cos(T1(1))*cos(T3(1)+T4(1))*sin(T1(1))*sin(T4(1))*a2*a3*m4 +
cos(T1(1))*cos(T4(1))*sin(T1(1))*sin(T3(1)+T4(1))*a2*a3*m4);
ME42=(-cos(T2(1))*cos(T4(1))*a2*a3*m4);
ME43=(m4*cos(T1(1))^2*sin(T3(1)+T4(1))^2*a2^2 +
sin(T4(1))*a3*m4*cos(T1(1))^2*sin(T3(1)+T4(1))*a2 +
m4*sin(T1(1))^2*sin(T3(1)+T4(1))^2*a2^2 +
sin(T4(1))*m4*sin(T1(1))^2*sin(T3(1)+T4(1))*a2);
ME44=(m4*cos(T1(1))^2*sin(T3(1)+T4(1))^2*a2^2 +
2*m4*cos(T1(1))^2*sin(T3(1)+T4(1))*sin(T4(1))*a2*a3 +
m4*cos(T1(1))^2*sin(T4(1))^2*a3^2 + m4*cos(T4(1))^2*a3^2 +
m4*sin(T1(1))^2*sin(T3(1)+T4(1))^2*a2^2 +
2*m4*sin(T1(1))^2*sin(T3(1)+T4(1))*sin(T4(1))*a2 +
m4*sin(T1(1))^2*sin(T4(1))^2);
ME45=0;

ME51=(cos(T1(1)+T5(1))^2*Me5*a5^2 + Me5*sin(T1(1)+T5(1))^2*a5^2 +
Me5*sin(T1(1))*sin(T1(1)+T5(1))*a1*a5 +
Me5*sin(T1(1))*sin(T1(1)+T5(1))*a4*a5 +
cos(T1(1))*cos(T1(1)+T5(1))*cos(T2(1)+T3(1))*Me5*a5 +
cos(T1(1))*cos(T1(1)+T5(1))*Me5*a1*a5 +
cos(T1(1))*cos(T1(1)+T5(1))*Me5*a4*a5 +
cos(T1(1))*cos(T1(1)+T5(1))*cos(T2(1))*Me5*a2*a5 +
cos(T2(1))*Me5*sin(T1(1))*sin(T1(1)+T5(1))*a2*a5 +
cos(T2(1)+T3(1))*Me5*sin(T1(1))*sin(T1(1)+T5(1))*a3*a5);
ME52=(cos(T1(1))*Me5*sin(T1(1)+T5(1))*sin(T2(1))*a2*a5 cos(T1(1)+T5(1))*Me5*sin(T1(1))*sin(T2(1)+T3(1))*a5 cos(T1(1)+T5(1))*Me5*sin(T1(1))*sin(T2(1))*a2*a5 +
cos(T1(1))*Me5*sin(T1(1)+T5(1))*sin(T2(1)+T3(1))*a3*a5);
ME53=(cos(T1(1))*Me5*sin(T1(1)+T5(1))*sin(T2(1)+T3(1))*a3*a5 cos(T1(1)+T5(1))*Me5*sin(T1(1))*sin(T2(1)+T3(1))*a5);
ME54=0;
ME55=(Me5*cos(T1(1)+T5(1))^2*a5^2 + Me5*sin(T1(1)+T5(1))^2*a5^2);
ME=[ME11 ME12 ME13 ME14 ME15; ME21 ME22 ME23 ME24 ME25; ME31 ME32
ME33 ME34 ME35; ME41 ME42 ME43 ME44 ME45;ME51 ME52 ME53 ME54 ME55;];

%% %% goc tinh theo R
% gia tri ban dau cua bien khop T1
S1=(yr(1)-L*sin(Tr(1)))/(sqrt((yr(1)-L*sin(Tr(1)))^2+(xr(1)L*cos(Tr(1)))^2));
C1=((xr(1)-L*cos(Tr(1)))/(sqrt(yr(1)-L*sin(Tr(1)))^2+(xr(1)L*cos(Tr(1)))^2));
T1r(1)=atan2(S1,C1);
%gia tri ban dau cua bien khop T5
T5r(1)=Tr(1)-T1(1);
% gia tri ban dau cua bien khop T3
C3=((xr(1)*cos(T1(1))+yr(1)*sin(T1(1))-a4-a1-L*cos(T5(1)))^2+(zr(1)d5)^2-a3^2-a2^2)/(2*a2*a3);
S3=(sqrt(1-C3^2));
T3r(1)=atan2(S3,C3);
% gia tri ban dau cua bien khop T2
C2=(a3*S3*(zr(1)-d5)+(a3*S3+a2)*(xr(1)*cos(T1(1))+yr(1)*sin(T1(1))-a4a1-L*cos(T5(1))))/((a3*cos(T3r(1))+a2)^2+(a3*sin(T3r(1)))^2)
S2=(sqrt(1-C2^2))
T2r(1)=atan2(S2,C2)
% gia tri ban dau cua bien khop T4
T4r(1)=-(T2r(1)+T3r(1));
% gia tri van toc ban dau cua khop
dT1r(1)=0; dT2r(1)=0; dT3r(1)=0; dT4r(1)=0; dT5r(1)=0;
%% Dinh nghia sai so ban dau
e1r(1)= T1r(1)-T1(1);
e2r(1)= T2r(1)-T2(1);
e3r(1)= T3r(1)-T3(1);
e4r(1)= T4r(1)-T4(1);
e5r(1)= T5r(1)-T5(1);
%% gia toc goc
ddT1r(1)=0;
ddT2r(1)=0;
ddT3r(1)=0;
ddT4r(1)=0;
ddT5r(1)=0;
ddT1(1)=0;
ddT2(1)=0;
ddT3(1)=0;
ddT4(1)=0;
ddT5(1)=0;
%========vong lap chinh==========
n=6000;
for i=2:n
%thoi gian t (giay)
t(i)=(i-1)*dt;
% Tao qui dao la duong trong mat phang // mat xy
xr(i) = 0.5+0.25*cos(0.05*t(i));
yr(i) = 0.7+0.25*sin(0.05*t(i));
zr(i) = 0.4;
%goc phi
Ter(i)=atan2(xr(i),yr(i));
%van toc diem R tai thoi diem i
vrx(i)=(xr(i)-xr(i-1))/dt;
vry(i)=(yr(i)-yr(i-1))/dt;

vrz(i)=(zr(i)-zr(i-1))/dt;
%van toc diem E tai thoi diem i trung voi R
%vex(i)=vrx(i);
%vey(i)=vry(i);
%vez(i)= vrz(i);
%Te(i)=Ter(i);
% Van toc goc cac khop tai thoi diem i
dT1(i)=(vry(i)*cos(T1(i-1)) - vrx(i)*sin(T1(i-1)) - L*Ter(i1)*cos(T5(i-1)) + e2(i-1)*k2*cos(T1(i-1)) - e1(i-1)*k1*sin(T1(i-1)) L*eTe(i-1)*k4*cos(T5(i-1)))/(a1 + a4 + a3*cos(T2(i-1) + T3(i-1)) +
a2*cos(T2(i-1)));
dT2(i)=((vry(i) + e2(i-1)*k2)*(a2*sin(T1(i-1) + T3(i-1)) +
2*a1*sin(T1(i-1) - T2(i-1) - T3(i-1)) + a2*sin(T1(i-1) - 2*T2(i-1) T3(i-1)) + a3*sin(T1(i-1) - 2*T2(i-1) - 2*T3(i-1)) + a3*sin(T1(i-1) +
2*T2(i-1) + 2*T3(i-1)) + 2*a4*sin(T1(i-1) - T2(i-1) - T3(i-1)) +
2*a3*sin(T1(i-1)) + 2*a1*sin(T1(i-1) + T2(i-1) + T3(i-1)) +
2*a4*sin(T1(i-1) + T2(i-1) + T3(i-1)) + a2*sin(T1(i-1) - T3(i-1)) +
a2*sin(T1(i-1) + 2*T2(i-1) + T3(i-1))))/(2*a2*(a2*sin(T2(i-1) + T3(i1)) + 2*a1*sin(T3(i-1)) - a3*sin(T2(i-1)) + 2*a4*sin(T3(i-1)) a2*sin(T2(i-1) - T3(i-1)) + a3*sin(T2(i-1) + 2*T3(i-1)))) + ((vrx(i) +
e1(i-1)*k1)*(a2*cos(T1(i-1) + T3(i-1)) + 2*a1*cos(T1(i-1) - T2(i-1) T3(i-1)) + a2*cos(T1(i-1) - 2*T2(i-1) - T3(i-1)) + a3*cos(T1(i-1) 2*T2(i-1) - 2*T3(i-1)) + a3*cos(T1(i-1) + 2*T2(i-1) + 2*T3(i-1)) +
2*a4*cos(T1(i-1) - T2(i-1) - T3(i-1)) + 2*a3*cos(T1(i-1)) +
2*a1*cos(T1(i-1) + T2(i-1) + T3(i-1)) + 2*a4*cos(T1(i-1) + T2(i-1) +
T3(i-1)) + a2*cos(T1(i-1) - T3(i-1)) + a2*cos(T1(i-1) + 2*T2(i-1) +
T3(i-1))))/(2*a2*(a2*sin(T2(i-1) + T3(i-1)) + 2*a1*sin(T3(i-1)) a3*sin(T2(i-1)) + 2*a4*sin(T3(i-1)) - a2*sin(T2(i-1) - T3(i-1)) +
a3*sin(T2(i-1) + 2*T3(i-1)))) + (sin(T2(i-1) + T3(i-1))*(vrz(i) + e3(i1)*k3))/(a2*sin(T3(i-1))) + (L*(Ter(i-1) + eTe(i-1)*k4)*(a2*sin(T3(i-1)
+ T5(i-1)) - a2*sin(2*T2(i-1) + T3(i-1) - T5(i-1)) + a3*sin(2*T2(i-1) +
2*T3(i-1) + T5(i-1)) + 2*a3*sin(T5(i-1)) + 2*a1*sin(T2(i-1) + T3(i-1) +
T5(i-1)) + 2*a4*sin(T2(i-1) + T3(i-1) + T5(i-1)) - a2*sin(T3(i-1) T5(i-1)) - a3*sin(2*T2(i-1) + 2*T3(i-1) - T5(i-1)) - 2*a1*sin(T2(i-1) +
T3(i-1) - T5(i-1)) + a2*sin(2*T2(i-1) + T3(i-1) + T5(i-1)) 2*a4*sin(T2(i-1) + T3(i-1) - T5(i-1))))/(2*a2*(a2*sin(T2(i-1) + T3(i1)) + 2*a1*sin(T3(i-1)) - a3*sin(T2(i-1)) + 2*a4*sin(T3(i-1)) a2*sin(T2(i-1) - T3(i-1)) + a3*sin(T2(i-1) + 2*T3(i-1))));
dT3(i)=-((vrx(i) + e1(i-1)*k1)*((a3^2*cos(T1(i-1) - 2*T2(i-1) 2*T3(i-1)))/2 + (a3^2*cos(T1(i-1) + 2*T2(i-1) + 2*T3(i-1)))/2 +
a2^2*cos(T1(i-1)) + a3^2*cos(T1(i-1)) + (a2^2*cos(T1(i-1) - 2*T2(i1)))/2 + (a2^2*cos(T1(i-1) + 2*T2(i-1)))/2 + a2*a3*cos(T1(i-1) +
2*T2(i-1) + T3(i-1)) + a1*a2*cos(T1(i-1) + T2(i-1)) + a2*a3*cos(T1(i-1)
+ T3(i-1)) + a2*a4*cos(T1(i-1) + T2(i-1)) + a1*a3*cos(T1(i-1) - T2(i-1)
- T3(i-1)) + a2*a3*cos(T1(i-1) - 2*T2(i-1) - T3(i-1)) + a3*a4*cos(T1(i1) - T2(i-1) - T3(i-1)) + a1*a3*cos(T1(i-1) + T2(i-1) + T3(i-1)) +
a3*a4*cos(T1(i-1) + T2(i-1) + T3(i-1)) + a1*a2*cos(T1(i-1) - T2(i-1)) +
a2*a3*cos(T1(i-1) - T3(i-1)) + a2*a4*cos(T1(i-1) - T2(i-1))))/
(a2*a3*(a2*sin(T2(i-1) + T3(i-1)) + 2*a1*sin(T3(i-1)) - a3*sin(T2(i-1))
+ 2*a4*sin(T3(i-1)) - a2*sin(T2(i-1) - T3(i-1)) + a3*sin(T2(i-1) +
2*T3(i-1)))) - ((vrz(i) + e3(i-1)*k3)*(a3*sin(T2(i-1) + T3(i-1)) +
a2*sin(T2(i-1))))/(a2*a3*sin(T3(i-1))) - ((vry(i) + e2(i1)*k2)*((a3^2*sin(T1(i-1) - 2*T2(i-1) - 2*T3(i-1)))/2 + (a3^2*sin(T1(i1) + 2*T2(i-1) + 2*T3(i-1)))/2 + a2^2*sin(T1(i-1)) + a3^2*sin(T1(i-1))
+ (a2^2*sin(T1(i-1) - 2*T2(i-1)))/2 + (a2^2*sin(T1(i-1) + 2*T2(i-1)))/2
+ a2*a3*sin(T1(i-1) + 2*T2(i-1) + T3(i-1)) + a1*a2*sin(T1(i-1) + T2(i-

1)) + a2*a3*sin(T1(i-1) + T3(i-1)) + a2*a4*sin(T1(i-1) + T2(i-1)) +
a1*a3*sin(T1(i-1) - T2(i-1) - T3(i-1)) + a2*a3*sin(T1(i-1) - 2*T2(i-1)
- T3(i-1)) + a3*a4*sin(T1(i-1) - T2(i-1) - T3(i-1)) + a1*a3*sin(T1(i-1)
+ T2(i-1) + T3(i-1)) + a3*a4*sin(T1(i-1) + T2(i-1) + T3(i-1)) +
a1*a2*sin(T1(i-1) - T2(i-1)) + a2*a3*sin(T1(i-1) - T3(i-1)) +
a2*a4*sin(T1(i-1) - T2(i-1))))/(a2*a3*(a2*sin(T2(i-1) + T3(i-1)) +
2*a1*sin(T3(i-1)) - a3*sin(T2(i-1)) + 2*a4*sin(T3(i-1)) - a2*sin(T2(i1) - T3(i-1)) + a3*sin(T2(i-1) + 2*T3(i-1)))) - (L*(Ter(i) + eTe(i1)*k4)*(a3^2*sin(2*T2(i-1) + 2*T3(i-1) + T5(i-1)) - a2^2*sin(2*T2(i-1)
- T5(i-1)) - a3^2*sin(2*T2(i-1) + 2*T3(i-1) - T5(i-1)) +
2*a2^2*sin(T5(i-1)) + 2*a3^2*sin(T5(i-1)) + a2^2*sin(2*T2(i-1) + T5(i1)) - 2*a1*a3*sin(T2(i-1) + T3(i-1) - T5(i-1)) + 2*a2*a3*sin(2*T2(i-1)
+ T3(i-1) + T5(i-1)) - 2*a3*a4*sin(T2(i-1) + T3(i-1) - T5(i-1)) +
2*a1*a2*sin(T2(i-1) + T5(i-1)) + 2*a2*a3*sin(T3(i-1) + T5(i-1)) +
2*a2*a4*sin(T2(i-1) + T5(i-1)) - 2*a2*a3*sin(2*T2(i-1) + T3(i-1) T5(i-1)) + 2*a1*a3*sin(T2(i-1) + T3(i-1) + T5(i-1)) + 2*a3*a4*sin(T2(i1) + T3(i-1) + T5(i-1)) - 2*a1*a2*sin(T2(i-1) - T5(i-1)) 2*a2*a3*sin(T3(i-1) - T5(i-1)) - 2*a2*a4*sin(T2(i-1) - T5(i-1))))/
(2*a2*a3*(a2*sin(T2(i-1) + T3(i-1)) + 2*a1*sin(T3(i-1)) - a3*sin(T2(i1)) + 2*a4*sin(T3(i-1)) - a2*sin(T2(i-1) - T3(i-1)) + a3*sin(T2(i-1) +
2*T3(i-1))));
dT4(i)=-(dT2(i)+dT3(i));
dT5(i)=(sin(T1(i-1))*(vrx(i) + e1(i-1)*k1))/(a1 + a4 + a3*cos(T2(i-1)
+ T3(i-1)) + a2*cos(T2(i-1))) - (cos(T1(i-1))*(vry(i) + e2(i-1)*k2))/
(a1 + a4 + a3*cos(T2(i-1) + T3(i-1)) + a2*cos(T2(i-1))) + ((Ter(i) +
eTe(i-1)*k4)*(a1 + a4 + a3*cos(T2(i-1) + T3(i-1)) + L*cos(T5(i-1)) +
a2*cos(T2(i-1))))/(a1 + a4 + a3*cos(T2(i-1)+ T3(i-1)) + a2*cos(T2(i1)));
%% Gia tri bien khop tai thoi diem i
T1(i)=T1(i-1)+dT1(i)*dt;
T2(i)=T2(i-1)+dT2(i)*dt;
T3(i)=T3(i-1)+dT3(i)*dt;
T4(i)=-(T2(i)+T3(i));
T4(i)=T4(i-1)+dT4(i)*dt;
T5(i)=T5(i-1)+dT5(i)*dt;
%% Vi tri diem E tai thoi diem i duoc dung hinh bang hinh hoc
xe(i)=(a1+a2*cos(T2(i))+a3*cos(T2(i)+T3(i))+a4)*cos(T1(i))
+L*cos(T1(i)+T5(i));
ye(i)=(a1+a2*cos(T2(i))+a3*cos(T2(i)+T3(i))+a4)*sin(T1(i))
+L*sin(T1(i)+T5(i));
ze(i)=a2*sin(T2(i))+a3*sin(T2(i)+T3(i))+d5;
Te(i)=T5(i)+T1(i);
%% Tinh sai so
e1(i)=xr(i)-xe(i);
e2(i)=yr(i)-ye(i);
e3(i)=zr(i)-ze(i);
eTe(i)=Ter(i)-Te(i);
end;
%=================het vong lap================
hold on;
% ve tay may
plot3(xr,yr,zr,'r',xe,ye,ze,'b'); xlabel('hanh do (m)');
ylabel('tung do (m)'); zlabel('cao do (m)');

title('H1: Bieu do quy dao dieu khien va quy dao mong muon');
legend('Desire','Actual');

Sponsor Documents

Or use your account on DocShare.tips

Hide

Forgot your password?

Or register your new account on DocShare.tips

Hide

Lost your password? Please enter your email address. You will receive a link to create a new password.

Back to log-in

Close