clc,clear
h=1.69;
M=82;
g=9.81;
m1=3.25.*M./100;
m2=1.87.*M./100;
l1=17.2.*h./100;
l2=15.7.*h./100;
r1=43.6.*l1./100;
r2=43.*l2./100;
IG1=m1.*(0.542)^2+m1.*(r1)^2;
IG2=m2.*(0.526)^2+m2.*(r2)^2;
t=[ 0 0.033 0.067 0.100 0.133 0.167 0.200 0.233 0.267 0.300 0.333 0.367 0.400 0.433 0.467 0.500 0.533 0.567 0.600 0.633 ...
0.667 0.700 0.733 0.767 0.800 0.833 0.867 0.900 0.933 0.967 1.000 1.033 1.067 1.100 1.133 1.167 ...
1.200 1.233 1.267 1.300 1.333 1.367 1.400 1.433 1.467 1.500];
theata1=[ 15.418 15.418 15.418 15.418 15.598 15.618 15.630 15.630 ...
15.623 15.652 16.450 16.506 16.695 16.963 17.082 17.282 17.380 17.396 17.444 17.316 ...
16.958 16.998 17.595 17.667 17.713 17.393 16.943 17.506 17.672 17.475 17.084 ...
17.332 17.444 17.470 17.599 17.481 17.301 17.227 17.247 17.214 17.177 17.004 16.993 ...
17.075 17.028 17.004 ];
dt=0.033;
dtheata1=diff(theata1)./dt;
dtheata1=[dtheata1 0];
theata2=[ 3.627 4.191 4.474 4.640 4.884 4.947 5.106 5.101 5.224 5.316 5.454 5.675 6.036 ...
6.338 6.788 7.120 7.571 7.729 7.995 8.062 8.177 8.358 8.435 8.703 9.159 10.035 11.659 14.597 18.011 ...
23.129 25.319 26.081 26.842 26.496 26.842 26.084 26.910 26.772 26.495 26.290 25.572 25.663 25.858 25.429 25.301 ...
24.753];
dtheata2=diff(theata2)./dt;
dtheata2=[dtheata2 0];
ddtheata1=diff(dtheata1)./dt;
ddtheata1=[ddtheata1 0 ];
ddtheata2=diff(dtheata2)./dt;
ddtheata2=[ddtheata2 0 ];
aG1x=r1.*ddtheata1.*cos(theata1)-r1.*((dtheata1).^2).*sin(theata1);
aG1y=r1.*ddtheata1.*sin(theata1)+r1.*((dtheata1).^2).*cos(theata1);
aG2x=l1.*ddtheata1.*cos(theata1)-l1.*((dtheata1).^2).*sin(theata1)+r2.*(ddtheata1+ddtheata2).*cos(theata1+theata2)-r2.*((dtheata2+dtheata1).^2).*sin(theata1+theata2);
aG2y=l1.*ddtheata1.*cos(theata1)+l1.*((dtheata1).^2).*cos(theata1)+r2.*(ddtheata1+ddtheata2).*sin(theata1+theata2)+r2.*((dtheata2+dtheata1).^2).*cos(theata1+theata2);
syms f1x f2x f1y f2y tou1 tou2;
eq1=m1.*aG1x-f1x+f2x;
eq2=m1.*aG1y-f1y-f2y-m1.*g;
eq3=IG1.*ddtheata1+f1x.*r1.*cos(theata1)+f1y.*r1.*sin(theata1)+f2x.*(l1-r1).*cos(theata1)-f2y.*(l1-r1).*sin(theata1)-tou1-tou2;
eq4=m2.*aG2x-f2x;
eq5=m2.*aG2y-m2.*g+f2y;
eq6=IG2.*(ddtheata1+ddtheata2)+tou2+f2x.*r2.*cos(theata1+theata2)-f2y.*r2.*sin(theata1+theata2);
[f1_x, f2_x, f1_y, f2_y, tou_1, tou_2]= solve([eq1, eq2, eq3, eq4, eq5, eq6] , [f1x, f2x, f1y, f2y, tou1, tou2])
