All All Duongtron N XR Yr

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);
% 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);
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);
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;
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);
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);
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)
T4(1)=-(T2(1)+T3(1));
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))*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)+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))*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 +
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)+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)+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))*cos(T2(1)+T3(1))*Me5*sin(T1(1))*sin(T2(1))*a2*a3 +
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)+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))^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*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 +
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)+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

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));
%gia tri ban dau cua bien khop T5
T5r(1)=Tr(1)-T1(1);
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));
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)
T4r(1)=-(T2r(1)+T3r(1));
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');

All All Duongtron N XR Yr

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

All All Duongtron N XR Yr

Uploaded by

Copyright:

Available Formats

clear all;

% lap trinh track theo qui dao dtron

% goc phi diem E ban dau

ME11=(1/2a1^2m1 + cos(T1(1))^2Me5a1^2 + cos(T1(1))^2Me5a4^2 +

ME41=(cos(T1(1))sin(T1(1))sin(T4(1))a1a3m4 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))a2m4 cos(T1(1))cos(T4(1))sin(T1(1))sin(T3(1)+T4(1))a2m4 cos(T1(1))sin(T1(1))sin(T4(1))a1*m4 +

%% %% goc tinh theo R

1)) + a2a3sin(T1(i-1) + T3(i-1)) + a2a4sin(T1(i-1) + T2(i-1)) +

You might also like