function  dinamicm
clc, close
disp('   This program is my Dynamic machine project in mechanical engineering department ')
disp('   of Amirkabir University of Technology. (www.aut.ac.ir)                          ')
disp('   By : Morteza Ahmadi Najafabadi                                                  ')
disp('                                                                                   ')
disp('   For more information please read help.pdf                                       ')

disp('   Equations is produced by dinsolve.m                                             ')



%% INPUT PARAMETER :
%  
%   r_00 r_1 r_2 r_3 r_4 r_5 r_7 r_8  a_01 ao_01 aoo_01 t_s t_step t_e rr
%
%% MAIN PROGRAM


u=0;d=0;l=0;r=0;
rr=1;
i=1; t=0;
r_00=2;r_0(i)=r_00;
r_1=8;  
aoo_01=1;  ao_01=2;  a_01=90;
aoo_1(i)=1; ao_1(i)=aoo_01*t+ao_01; a_1(i)= aoo_01/2*t^2 + ao_01*t + a_01*pi/180;
r_2=2;
r_3=8;
a_3(1)=asin(1/2*(r_2^2*r_1^2+r_1*cos(a_1(i))*((-1+cos(a_1(i))^2)*r_1^2*(4*r_1^2*cos(a_1(i))^2*r_0^2+4*r_1*cos(a_1(i))*r_0*r_2^2-4*r_1^3*cos(a_1(i))*r_0-4*r_1*cos(a_1(i))*r_0^3+4*r_3^2*r_1*cos(a_1(i))*r_0-2*r_3^2*r_2^2-2*r_2^2*r_0^2+r_2^4+r_1^4-2*r_3^2*r_0^2-2*r_3^2*r_1^2-2*r_2^2*r_1^2+2*r_0^2*r_1^2+r_3^4+r_0^4))^(1/2)-r_3^2*r_1^2-r_0^2*r_1^2-r_1^4-r_0*((-1+cos(a_1(i))^2)*r_1^2*(4*r_1^2*cos(a_1(i))^2*r_0^2+4*r_1*cos(a_1(i))*r_0*r_2^2-4*r_1^3*cos(a_1(i))*r_0-4*r_1*cos(a_1(i))*r_0^3+4*r_3^2*r_1*cos(a_1(i))*r_0-2*r_3^2*r_2^2-2*r_2^2*r_0^2+r_2^4+r_1^4-2*r_3^2*r_0^2-2*r_3^2*r_1^2-2*r_2^2*r_1^2+2*r_0^2*r_1^2+r_3^4+r_0^4))^(1/2)+2*r_1^3*cos(a_1(i))*r_0-2*r_1^3*cos(a_1(i))^3*r_0+r_1^4*cos(a_1(i))^2-r_1^2*cos(a_1(i))^2*r_2^2+r_1^2*cos(a_1(i))^2*r_3^2+r_1^2*cos(a_1(i))^2*r_0^2)/(r_1^2+r_0^2-2*r_1*cos(a_1(i))*r_0)/r_1/sin(a_1(i))/r_3);
a_2(i) = asin(1/2*(r_1^2*cos(a_1(i))^2*r_0^2-r_1^2*cos(a_1(i))^2*r_3^2+r_1^2*cos(a_1(i))^2*r_2^2+r_0*((-1+cos(a_1(i))^2)*r_1^2*(4*r_1^2*cos(a_1(i))^2*r_0^2+4*r_1*cos(a_1(i))*r_0*r_2^2-4*r_1^3*cos(a_1(i))*r_0-4*r_1*cos(a_1(i))*r_0^3+4*r_3^2*r_1*cos(a_1(i))*r_0-2*r_3^2*r_2^2-2*r_2^2*r_0^2+r_2^4+r_1^4-2*r_3^2*r_0^2-2*r_3^2*r_1^2-2*r_2^2*r_1^2+2*r_0^2*r_1^2+r_3^4+r_0^4))^(1/2)-r_2^2*r_1^2-r_1*cos(a_1(i))*((-1+cos(a_1(i))^2)*r_1^2*(4*r_1^2*cos(a_1(i))^2*r_0^2+4*r_1*cos(a_1(i))*r_0*r_2^2-4*r_1^3*cos(a_1(i))*r_0-4*r_1*cos(a_1(i))*r_0^3+4*r_3^2*r_1*cos(a_1(i))*r_0-2*r_3^2*r_2^2-2*r_2^2*r_0^2+r_2^4+r_1^4-2*r_3^2*r_0^2-2*r_3^2*r_1^2-2*r_2^2*r_1^2+2*r_0^2*r_1^2+r_3^4+r_0^4))^(1/2)+2*r_1^3*cos(a_1(i))*r_0-2*r_1^3*cos(a_1(i))^3*r_0-r_0^2*r_1^2+r_3^2*r_1^2-r_1^4+r_1^4*cos(a_1(i))^2)/(r_1^2+r_0^2-2*r_1*cos(a_1(i))*r_0)/r_1/sin(a_1(i))/r_2);
r_4=4;
r_5=10;
r_7=2;
r_8=5;
a_4(i)=asin(1/2*(r_2^2*sin(a_2(i))-sin(a_2(i))*r_8^2+r_4^2*sin(a_2(i))+(-cos(a_2(i))^2*(r_4+r_2+r_8)*(r_2+r_4-r_8)*(r_2-r_8-r_4)*(r_8-r_4+r_2))^(1/2))/r_2/r_4);
a_5(i) =asin((r_3*sin(a_3(i))-r_4*sin(a_4(i))+r_7)/r_5);
a_8(i) =-asin(1/2*(sin(a_2(i))*r_8^2+r_2^2*sin(a_2(i))-r_4^2*sin(a_2(i))-(-cos(a_2(i))^2*(r_4+r_2+r_8)*(r_2+r_4-r_8)*(r_2-r_8-r_4)*(r_8-r_4+r_2))^(1/2))/r_2/r_8);
r_6(i) =-r_3*cos(a_3(i))+r_4*cos(a_4(i))+r_5*(-(r_5+r_7-r_4*sin(a_4(i))+r_3*sin(a_3(i)))*(-r_5+r_7-r_4*sin(a_4(i))+r_3*sin(a_3(i)))/r_5^2)^(1/2);


t_s=0; t_step=.05;t_e=10;
for t=t_s:t_step:t_e

i=i+1;  
r_0(i)=r_00 ;  
aoo_1(i)=1;
ao_1(i)=aoo_01*t;
a_1(i)= aoo_01/2*t^2 + ao_01*t + a_01*pi/180;
a_2(i)= asin(1/2.*(r_1.^2.*cos(a_1(i)).^2.*r_0.^2-r_1.^2.*cos(a_1(i)).^2.*r_3.^2+r_1.^2.*cos(a_1(i)).^2.*r_2.^2+r_0.*((-1+cos(a_1(i)).^2).*r_1.^2.*(4.*r_1.^2.*cos(a_1(i)).^2.*r_0.^2+4.*r_1.*cos(a_1(i)).*r_0.*r_2.^2-4.*r_1.^3.*cos(a_1(i)).*r_0-4.*r_1.*cos(a_1(i)).*r_0.^3+4.*r_3.^2.*r_1.*cos(a_1(i)).*r_0-2.*r_3.^2.*r_2.^2-2.*r_2.^2.*r_0.^2+r_2.^4+r_1.^4-2.*r_3.^2.*r_0.^2-2.*r_3.^2.*r_1.^2-2.*r_2.^2.*r_1.^2+2.*r_0.^2.*r_1.^2+r_3.^4+r_0.^4)).^(1/2)-r_2.^2.*r_1.^2-r_1.*cos(a_1(i)).*((-1+cos(a_1(i)).^2).*r_1.^2.*(4.*r_1.^2.*cos(a_1(i)).^2.*r_0.^2+4.*r_1.*cos(a_1(i)).*r_0.*r_2.^2-4.*r_1.^3.*cos(a_1(i)).*r_0-4.*r_1.*cos(a_1(i)).*r_0.^3+4.*r_3.^2.*r_1.*cos(a_1(i)).*r_0-2.*r_3.^2.*r_2.^2-2.*r_2.^2.*r_0.^2+r_2.^4+r_1.^4-2.*r_3.^2.*r_0.^2-2.*r_3.^2.*r_1.^2-2.*r_2.^2.*r_1.^2+2.*r_0.^2.*r_1.^2+r_3.^4+r_0.^4)).^(1/2)+2.*r_1.^3.*cos(a_1(i)).*r_0-2.*r_1.^3.*cos(a_1(i)).^3.*r_0-r_0.^2.*r_1.^2+r_3.^2.*r_1.^2-r_1.^4+r_1.^4.*cos(a_1(i)).^2)/(r_1.^2+r_0.^2-2.*r_1.*cos(a_1(i)).*r_0)/r_1/sin(a_1(i))/r_2);
%a_2(i) =asin(1/2.*(r_1.^2.*cos(a_1(i)).^2.*r_0.^2-r_1.^2.*cos(a_1(i)).^2.*r_3.^2+r_1.^2.*cos(a_1(i)).^2.*r_2.^2-r_2.^2.*r_1.^2-r_0.^2.*r_1.^2+r_3.^2.*r_1.^2+r_1.*cos(a_1(i)).*((-1+cos(a_1(i)).^2).*r_1.^2.*(4.*r_1.^2.*cos(a_1(i)).^2.*r_0.^2+4.*r_1.*cos(a_1(i)).*r_0.*r_2.^2-4.*r_1.^3.*cos(a_1(i)).*r_0-4.*r_1.*cos(a_1(i)).*r_0.^3+4.*r_3.^2.*r_1.*cos(a_1(i)).*r_0-2.*r_3.^2.*r_2.^2-2.*r_2.^2.*r_0.^2+r_2.^4+r_1.^4-2.*r_3.^2.*r_0.^2-2.*r_3.^2.*r_1.^2-2.*r_2.^2.*r_1.^2+2.*r_0.^2.*r_1.^2+r_3.^4+r_0.^4)).^(1/2)-r_1.^4+r_1.^4.*cos(a_1(i)).^2-r_0.*((-1+cos(a_1(i)).^2).*r_1.^2.*(4.*r_1.^2.*cos(a_1(i)).^2.*r_0.^2+4.*r_1.*cos(a_1(i)).*r_0.*r_2.^2-4.*r_1.^3.*cos(a_1(i)).*r_0-4.*r_1.*cos(a_1(i)).*r_0.^3+4.*r_3.^2.*r_1.*cos(a_1(i)).*r_0-2.*r_3.^2.*r_2.^2-2.*r_2.^2.*r_0.^2+r_2.^4+r_1.^4-2.*r_3.^2.*r_0.^2-2.*r_3.^2.*r_1.^2-2.*r_2.^2.*r_1.^2+2.*r_0.^2.*r_1.^2+r_3.^4+r_0.^4)).^(1/2)+2.*r_1.^3.*cos(a_1(i)).*r_0-2.*r_1.^3.*cos(a_1(i)).^3.*r_0)/(r_1.^2+r_0.^2-2.*r_1.*cos(a_1(i)).*r_0)/r_1/sin(a_1(i))/r_2);
a_3t(i)=asin(1/2.*(r_2.^2.*r_1.^2+r_1.*cos(a_1(i)).*((-1+cos(a_1(i)).^2).*r_1.^2.*(4.*r_1.^2.*cos(a_1(i)).^2.*r_0.^2+4.*r_1.*cos(a_1(i)).*r_0.*r_2.^2-4.*r_1.^3.*cos(a_1(i)).*r_0-4.*r_1.*cos(a_1(i)).*r_0.^3+4.*r_3.^2.*r_1.*cos(a_1(i)).*r_0-2.*r_3.^2.*r_2.^2-2.*r_2.^2.*r_0.^2+r_2.^4+r_1.^4-2.*r_3.^2.*r_0.^2-2.*r_3.^2.*r_1.^2-2.*r_2.^2.*r_1.^2+2.*r_0.^2.*r_1.^2+r_3.^4+r_0.^4)).^(1/2)-r_3.^2.*r_1.^2-r_0.^2.*r_1.^2-r_1.^4-r_0.*((-1+cos(a_1(i)).^2).*r_1.^2.*(4.*r_1.^2.*cos(a_1(i)).^2.*r_0.^2+4.*r_1.*cos(a_1(i)).*r_0.*r_2.^2-4.*r_1.^3.*cos(a_1(i)).*r_0-4.*r_1.*cos(a_1(i)).*r_0.^3+4.*r_3.^2.*r_1.*cos(a_1(i)).*r_0-2.*r_3.^2.*r_2.^2-2.*r_2.^2.*r_0.^2+r_2.^4+r_1.^4-2.*r_3.^2.*r_0.^2-2.*r_3.^2.*r_1.^2-2.*r_2.^2.*r_1.^2+2.*r_0.^2.*r_1.^2+r_3.^4+r_0.^4)).^(1/2)+2.*r_1.^3.*cos(a_1(i)).*r_0-2.*r_1.^3.*cos(a_1(i)).^3.*r_0+r_1.^4.*cos(a_1(i)).^2-r_1.^2.*cos(a_1(i)).^2.*r_2.^2+r_1.^2.*cos(a_1(i)).^2.*r_3.^2+r_1.^2.*cos(a_1(i)).^2.*r_0.^2)/(r_1.^2+r_0.^2-2.*r_1.*cos(a_1(i)).*r_0)/r_1/sin(a_1(i))/r_3);
%a_3(i)=asin(1/2.*(r_2.^2.*r_1.^2-r_1.*cos(a_1(i)).*((-1+cos(a_1(i)).^2).*r_1.^2.*(4.*r_1.^2.*cos(a_1(i)).^2.*r_0.^2+4.*r_1.*cos(a_1(i)).*r_0.*r_2.^2-4.*r_1.^3.*cos(a_1(i)).*r_0-4.*r_1.*cos(a_1(i)).*r_0.^3+4.*r_3.^2.*r_1.*cos(a_1(i)).*r_0-2.*r_3.^2.*r_2.^2-2.*r_2.^2.*r_0.^2+r_2.^4+r_1.^4-2.*r_3.^2.*r_0.^2-2.*r_3.^2.*r_1.^2-2.*r_2.^2.*r_1.^2+2.*r_0.^2.*r_1.^2+r_3.^4+r_0.^4)).^(1/2)-r_3.^2.*r_1.^2-r_0.^2.*r_1.^2-r_1.^4+r_0.*((-1+cos(a_1(i)).^2).*r_1.^2.*(4.*r_1.^2.*cos(a_1(i)).^2.*r_0.^2+4.*r_1.*cos(a_1(i)).*r_0.*r_2.^2-4.*r_1.^3.*cos(a_1(i)).*r_0-4.*r_1.*cos(a_1(i)).*r_0.^3+4.*r_3.^2.*r_1.*cos(a_1(i)).*r_0-2.*r_3.^2.*r_2.^2-2.*r_2.^2.*r_0.^2+r_2.^4+r_1.^4-2.*r_3.^2.*r_0.^2-2.*r_3.^2.*r_1.^2-2.*r_2.^2.*r_1.^2+2.*r_0.^2.*r_1.^2+r_3.^4+r_0.^4)).^(1/2)+2.*r_1.^3.*cos(a_1(i)).*r_0-2.*r_1.^3.*cos(a_1(i)).^3.*r_0+r_1.^4.*cos(a_1(i)).^2-r_1.^2.*cos(a_1(i)).^2.*r_2.^2+r_1.^2.*cos(a_1(i)).^2.*r_3.^2+r_1.^2.*cos(a_1(i)).^2.*r_0.^2)/(r_1.^2+r_0.^2-2.*r_1.*cos(a_1(i)).*r_0)/r_1/sin(a_1(i))/r_3);

a_3(i)=a_3t(i);
if (a_3t(i)<a_3t(i-1))
    a_3(i) =pi-asin(1/2.*(r_2.^2.*r_1.^2+r_1.*cos(a_1(i)).*((-1+cos(a_1(i)).^2).*r_1.^2.*(4.*r_1.^2.*cos(a_1(i)).^2.*r_0.^2+4.*r_1.*cos(a_1(i)).*r_0.*r_2.^2-4.*r_1.^3.*cos(a_1(i)).*r_0-4.*r_1.*cos(a_1(i)).*r_0.^3+4.*r_3.^2.*r_1.*cos(a_1(i)).*r_0-2.*r_3.^2.*r_2.^2-2.*r_2.^2.*r_0.^2+r_2.^4+r_1.^4-2.*r_3.^2.*r_0.^2-2.*r_3.^2.*r_1.^2-2.*r_2.^2.*r_1.^2+2.*r_0.^2.*r_1.^2+r_3.^4+r_0.^4)).^(1/2)-r_3.^2.*r_1.^2-r_0.^2.*r_1.^2-r_1.^4-r_0.*((-1+cos(a_1(i)).^2).*r_1.^2.*(4.*r_1.^2.*cos(a_1(i)).^2.*r_0.^2+4.*r_1.*cos(a_1(i)).*r_0.*r_2.^2-4.*r_1.^3.*cos(a_1(i)).*r_0-4.*r_1.*cos(a_1(i)).*r_0.^3+4.*r_3.^2.*r_1.*cos(a_1(i)).*r_0-2.*r_3.^2.*r_2.^2-2.*r_2.^2.*r_0.^2+r_2.^4+r_1.^4-2.*r_3.^2.*r_0.^2-2.*r_3.^2.*r_1.^2-2.*r_2.^2.*r_1.^2+2.*r_0.^2.*r_1.^2+r_3.^4+r_0.^4)).^(1/2)+2.*r_1.^3.*cos(a_1(i)).*r_0-2.*r_1.^3.*cos(a_1(i)).^3.*r_0+r_1.^4.*cos(a_1(i)).^2-r_1.^2.*cos(a_1(i)).^2.*r_2.^2+r_1.^2.*cos(a_1(i)).^2.*r_3.^2+r_1.^2.*cos(a_1(i)).^2.*r_0.^2)/(r_1.^2+r_0.^2-2.*r_1.*cos(a_1(i)).*r_0)/r_1/sin(a_1(i))/r_3);
end
    
if((isreal(a_2(i))==0)||(isreal(a_3(i))==0))
     msgbox('error1','error')
     break
 end

ao_2(i)=-r_1.*ao_1(i).*sin(a_1(i)-a_3(i))/sin(a_2(i)-a_3(i))/r_2;
ao_3(i)=-r_1.*ao_1(i).*sin(a_2(i)-a_1(i))/r_3/sin(a_2(i)-a_3(i));
 
aoo_2(i) =(-r_1.*ao_1(i).^2.*cos(a_1(i)-a_3(i))-r_1.*aoo_1(i).*sin(a_1(i)-a_3(i))-r_2.*ao_2(i).^2.*cos(-a_3(i)+a_2(i))-r_3.*ao_3(i).^2.*cos(-a_3(i)+a_3(i)))./r_2./sin(-a_3(i)+a_2(i));
aoo_3(i) =(r_1.*ao_1(i).^2.*cos(-a_2(i)+a_1(i))+r_1.*aoo_1(i).*sin(-a_2(i)+a_1(i))+r_2.*ao_2(i).^2+r_3.*ao_3(i).^2.*cos(-a_2(i)+a_3(i)))./r_3./sin(-a_3(i)+a_2(i));

%-----
a_4(i)=asin(1/2*(r_2^2*sin(a_2(i))-sin(a_2(i))*r_8^2+r_4^2*sin(a_2(i))+(-cos(a_2(i))^2*(r_4+r_2+r_8)*(r_2+r_4-r_8)*(r_2-r_8-r_4)*(r_8-r_4+r_2))^(1/2))/r_2/r_4);
%a_41(i)= -asin(1/2*(r_2^2*sin(a_2(i))-sin(a_2(i))*r_8^2+r_4^2*sin(a_2(i))-(-cos(a_2(i))^2*(r_4+r_2+r_8)*(r_2+r_4-r_8)*(r_2-r_8-r_4)*(r_8-r_4+r_2))^(1/2))/r_2/r_4);
a_8(i) =-asin(1/2*(sin(a_2(i))*r_8^2+r_2^2*sin(a_2(i))-r_4^2*sin(a_2(i))-(-cos(a_2(i))^2*(r_4+r_2+r_8)*(r_2+r_4-r_8)*(r_2-r_8-r_4)*(r_8-r_4+r_2))^(1/2))/r_2/r_8);
%a_81(i)=-asin(1/2*(sin(a_2(i))*r_8^2+r_2^2*sin(a_2(i))-r_4^2*sin(a_2(i))+(-cos(a_2(i))^2*(r_4+r_2+r_8)*(r_2+r_4-r_8)*(r_2-r_8-r_4)*(r_8-r_4+r_2))^(1/2))/r_2/r_8);
if((isreal(a_4(i))==0)||(isreal(a_8(i))==0))
     msgbox('error2','error')
     break
 end
 
ao_4(i)=-r_2*ao_2(i)*(sin(a_2(i)-a_8(i))-sin(a_8(i)-a_2(i)))/r_4/(sin(a_4(i)-a_8(i))-sin(a_8(i)-a_4(i)));
ao_8(i)=-r_2*ao_2(i)*(sin(a_4(i)-a_2(i))-sin(a_2(i)-a_4(i)))/r_8/(sin(a_4(i)-a_8(i))-sin(a_8(i)-a_4(i)));

aoo_4(i) =(r_2*ao_2(i)^2*cos(a_2(i)-a_8(i))+r_2*aoo_2(i)*sin(a_2(i)-a_8(i))+r_4*ao_4(i)^2*cos(a_8(i)-a_4(i))+r_4*ao_4(i)^2*cos(a_4(i)-a_8(i))+2*r_8*ao_8(i)^2+r_2*ao_2(i)^2*cos(a_8(i)-a_2(i))-r_2*aoo_2(i)*sin(a_8(i)-a_2(i)))/r_4/(-sin(a_4(i)-a_8(i))+sin(a_8(i)-a_4(i)));
aoo_8(i) =(r_2*ao_2(i)^2*cos(a_4(i)-a_2(i))-r_2*aoo_2(i)*sin(a_4(i)-a_2(i))+2*r_4*ao_4(i)^2+r_8*ao_8(i)^2*cos(a_4(i)-a_8(i))+r_2*ao_2(i)^2*cos(a_2(i)-a_4(i))+r_2*aoo_2(i)*sin(a_2(i)-a_4(i))+r_8*ao_8(i)^2*cos(a_8(i)-a_4(i)))/r_8/(sin(a_4(i)-a_8(i))-sin(a_8(i)-a_4(i)));

%----------------
a_5(i) =asin((r_3*sin(a_3(i))-r_4*sin(a_4(i))+r_7)/r_5);
r_6(i) =-r_3*cos(a_3(i))+r_4*cos(a_4(i))+r_5*(-(r_5+r_7-r_4*sin(a_4(i))+r_3*sin(a_3(i)))*(-r_5+r_7-r_4*sin(a_4(i))+r_3*sin(a_3(i)))/r_5^2)^(1/2);
t6=-(r_5+r_7-r_4*sin(a_4(i))+r_3*sin(a_3(i)))*(-r_5+r_7-r_4*sin(a_4(i))+r_3*sin(a_3(i)))/r_5^2;
if(t6<0)
     msgbox('error3','error')
     break
 end

if((isreal(a_5(i))==0)||(isreal(r_6(i))==0))
     msgbox('error3','error')
     break
 end
 
ao_5(i) =(r_3*ao_3(i)*cos(a_3(i))-r_4*ao_4(i)*cos(a_4(i)))/r_5/cos(a_5(i));
ro_6(i) =(ao_3(i)*sin(a_2(i))-1/cos(a_5(i))*sin(a_5(i))*ao_3(i)*cos(a_3(i)))*r_3-r_4*ao_4(i)*sin(a_4(i))+1/cos(a_5(i))*sin(a_5(i))*r_4*ao_4(i)*cos(a_4(i));

aoo_5(i) =-(r_3*ao_3(i)^2*sin(a_3(i))-r_3*aoo_3(i)*cos(a_3(i))-r_4*ao_4(i)^2*sin(a_4(i))+r_4*aoo_4(i)*cos(a_4(i))-r_5*ao_5(i)^2*sin(a_5(i)))/r_5/cos(a_5(i));
roo_6(i) =1/2*(r_3*ao_3(i)^2*cos(a_3(i)-a_5(i))+r_3*aoo_3(i)*sin(a_3(i)-a_5(i))-r_4*ao_4(i)^2*cos(a_4(i)-a_5(i))-r_4*aoo_4(i)*sin(a_4(i)-a_5(i))-2*r_5*ao_5(i)^2+r_3*ao_3(i)^2*cos(a_5(i)-a_3(i))-r_3*aoo_3(i)*sin(a_5(i)-a_3(i))-r_4*ao_4(i)^2*cos(a_5(i)-a_4(i))+r_4*aoo_4(i)*sin(a_5(i)-a_4(i)))/cos(a_5(i));

 
 

if(u<(r_1*sin(a_1(i))+r_8*sin(a_8(i))))
     u=r_1*sin(a_1(i))+r_8*sin(a_8(i));
end
if(d > (r_1*sin(a_1(i))))
     d=r_1*sin(a_1(i));
 end
if(l > (r_1*cos(a_1(i))))
     l=r_1*cos(a_1(i));
 end
 if(r < (r_0(i)+r_6(i)))
     r=r_0(i)+r_6(i);
 end

 
end


i=1;
for t=t_s:t_step:t_e
    i=i+1;
plot( [0 r_1*cos(a_1(i)) r_1*cos(a_1(i))+r_2*cos(a_2(i)) r_1*cos(a_1(i))+r_2*cos(a_2(i))+r_3*cos(a_3(i))] ,[0 r_1*sin(a_1(i)) r_1*sin(a_1(i))+r_2*sin(a_2(i)) r_1*sin(a_1(i))+r_2*sin(a_2(i))+r_3*sin(a_3(i))],'-or')
hold on
plot( [r_1*cos(a_1(i))+r_2*cos(a_2(i)) r_1*cos(a_1(i))+r_2*cos(a_2(i))+r_4*cos(a_4(i)) r_1*cos(a_1(i))+r_2*cos(a_2(i))+r_4*cos(a_4(i))+r_5*cos(a_5(i))] ,[r_1*sin(a_1(i))+r_2*sin(a_2(i)) r_1*sin(a_1(i))+r_2*sin(a_2(i))+r_4*sin(a_4(i)) r_1*sin(a_1(i))+r_2*sin(a_2(i))+r_4*sin(a_4(i))+r_5*sin(a_5(i))],'-or')
plot( [r_1*cos(a_1(i)) r_1*cos(a_1(i))+r_8*cos(a_8(i))] ,[r_1*sin(a_1(i)) r_1*sin(a_1(i))+r_8*sin(a_8(i))],'-or')
plot( [r_0(i)+r_6(i)-1 r_0(i)+r_6(i)+1] ,[r_7-rr r_7-rr],'-k')
do=0:.1:2*pi;
x=rr*cos(do);
y=rr*sin(do);
plot(r_1*cos(a_1(i))+r_2*cos(a_2(i))+r_4*cos(a_4(i))+r_5*cos(a_5(i))+x,r_1*sin(a_1(i))+r_2*sin(a_2(i))+r_4*sin(a_4(i))+r_5*sin(a_5(i))+y,'-r' ) 

xlim([l-1 r+rr+1])
ylim([d-1 u+1])

hold off
pause (.1)
end

t=t_s:t_step:t_e+t_step;
plot(t,ao_1)
xlabel('t(s)');ylabel('\omega (rad/s)');title('\omega_2');pause
plot(t,ao_2)
xlabel('t(s)');ylabel('\omega (rad/s)');title('\omega_3');pause
plot(t,ao_3)
xlabel('t(s)');ylabel('\omega (rad/s)');title('\omega_4');pause
plot(t,ao_5)
xlabel('t(s)');ylabel('\omega (rad/s)');title('\omega_5');pause


plot(t,aoo_1)
xlabel('t(s)');ylabel('\alpha (rad/s^2)');title('\alpha_2');pause
plot(t,aoo_2)
xlabel('t(s)');ylabel('\alpha (rad/s^2)');title('\alpha_3');pause
plot(t,aoo_3)
xlabel('t(s)');ylabel('\alpha (rad/s^2)');title('\alpha_4');pause
plot(t,aoo_5)
xlabel('t(s)');ylabel('\alpha (rad/s^2)');title('\alpha_5');pause
plot(t,roo_6)
xlabel('t(s)');ylabel('\alpha (rad/s^2)');title('a_6');pause


plot(t,r_1*ao_1)
xlabel('t(s)');ylabel('V(m/s)');title('V_A');pause
plot(t,r_3*ao_3)
xlabel('t(s)');ylabel('V(m/s)');title('V_B');pause
plot(t,r_3*ao_3)
xlabel('t(s)');ylabel('V(m/s)');title('V_C');pause
%????????????
plot(t,(((-r_3.*ao_3.*sin(a_3)).^2 + (r_3.*ao_3.*cos(a_3)).^2 ).^(1/2) ).^(1/2),'--k')
xlabel('t(s)');ylabel('V(m/s)');title('V_D');pause
hold on
%???????????
plot(t,( (-r_1.*ao_1.*sin(a_1) - r_2.*ao_2.*sin(a_2) - r_4.*ao_4.*sin(a_4) - r_5.*ao_5.*sin(a_5) ).^2 + (r_1.*ao_1.*cos(a_1) + r_2.*ao_2.*cos(a_2) + r_4.*ao_4.*cos(a_4) + r_5.*ao_5.*cos(a_5)).^2 ).^(1/2),'--k')
xlabel('t(s)');ylabel('V(m/s)');title('V_D');pause
hold on
plot(t,( +r_3.*ao_3.*sin(a_3) - r_4.*ao_4.*sin(a_4) - r_5.*ao_5.*sin(a_5) ),'--g')
xlabel('t(s)');ylabel('V(m/s)');title('V_D');pause
hold on
plot(t,ro_6,'-r')
xlabel('t(s)');ylabel('V(m/s)');title('V_D');pause
hold off


plot(t,r_1*aoo_1)
xlabel('t(s)');ylabel('a(m/s^2)');title('a_A');pause
plot(t,r_1*aoo_1+r_2*ao_2)
xlabel('t(s)');ylabel('a(m/s^2)');title('a_B');pause
hold on
plot(t,r_1*ao_1+r_2*ao_2+r_4*ao_4,'--r')
xlabel('t(s)');ylabel('a(m/s^2)');title('a_C?');pause
hold on
plot(t,r_3*ao_3+r_4*ao_4+r_5*ao_5)
xlabel('t(s)');ylabel('a(m/s^2)');title('a_D?');pause
hold on
plot(t,ro_6,'-r')
xlabel('t(s)');ylabel('a(m/s^2)');title('a_D');pause





close