% kinematika - reseni rovinnych mechanismu vektorovou metodou

close all; clear all; format compact; format short;

global b1 beta1 b2 beta2 b3 beta3 b4 beta4 b5 beta5 b6 beta6 b7 beta7 b8 beta8 b9 beta9 delta alfa gama b10 b11 b12 b13 b14 b15 b16 b17 b18 beta10 beta11 beta12 beta13 beta14 beta15 beta16 beta17 beta18 beta19
global b1t beta1t b2t beta2t b3t beta3t b4t beta4t b5t beta5t b6t beta6t b7t beta7t b8t beta8t b9t beta9t

% Rozmery mechanismu
XO=47.4e-3; YO=10e-3; AC=142.07e-3; CP=158.808e-3; CD=45.576e-3; DE=142e-3; BE=45.6e-3; ABx=1e-3; ABy=0; delta=3.89*pi/180; %XO=47.4e-3; YO=0; Plx=158.808e-3; Ply=0;  
% Konstanty ve vektorovych mnohouhelnicich
b3=CP; b4=AC; b5=CD; b6=DE; b7=BE; b8=ABx; b9=ABy; beta1=0; beta2=pi/2; beta8=3*pi/2; beta9=0;  
alfa=109.81*pi/180; gama=2.69*pi/180; b10=38.5e-3; b11=158.8e-3; b12=39.428e-3; b13=43.618e-3; b14=162.83e-3; b15=38.5e-3; b16=142e-3; b17=16.7e-3; b18=34.7e-3; b19=AC;


beta17=3*pi/2;
beta18=0;
npoloh=1;
QQ=[];
for QQ1=(50:1:100)*pi/180+pi
    for QQ2=(150:1:190)*pi/180+pi
    QQ=[QQ [QQ1;QQ2]];
    end
end
FI=QQ.';


% Odhady zavislych souradnic
b1=150e-3;
b2=130e-3;
beta3=200*pi/180;
beta6=250*pi/180;
beta10=45*pi/180;
beta11=270*pi/180;
beta12=270*pi/180;
beta16=270*pi/180;

% Vektor odhadu zavislych souradnic
z=[b1; b2; beta3; beta6;beta10;beta11;beta12;beta16];
 % Konec prvniho bloku
% Pole pro zavisle a jejich casove derivace

Z=[];
ZT=[];
nt=size(QQ,2);
for it=1:nt

   q=QQ(:,it);

   z=Kresic(z,q);
   Z=[Z z];

%    Definice promennych a derivaci
    beta4=q(1);
    beta7=q(2);

    b1=z(1);
    b2=z(2);
    beta3=z(3);
    beta6=z(4);
    beta10=z(5);
    beta11=z(6);
    beta12=z(7);
    beta16=z(8);

    beta5=beta3-delta;
    beta13=beta12-pi/2;
    beta14=beta3-gama;
    beta15=beta10+alfa;
    beta19=beta4-pi;

   % Nahrady cosinu a sinu
   c1=cos(beta1);    s1=sin(beta1);
   c2=cos(beta2);    s2=sin(beta2);
   c3=cos(beta3);    s3=sin(beta3);
   c4=cos(beta4);    s4=sin(beta4);
   c5=cos(beta5);    s5=sin(beta5);
   c6=cos(beta6);    s6=sin(beta6);
   c7=cos(beta7);    s7=sin(beta7);
   c8=cos(beta8);    s8=sin(beta8);
   c9=cos(beta9);    s9=sin(beta9);

   c10=cos(beta10);  s10=sin(beta10);
   c11=cos(beta11);  s11=sin(beta11);
   c12=cos(beta12);  s12=sin(beta12);
   c13=cos(beta13);  s13=sin(beta13);
   c14=cos(beta14);  s14=sin(beta14);
   c15=cos(beta15);  s15=sin(beta15);
   c16=cos(beta16);  s16=sin(beta16);
   c17=cos(beta17);  s17=sin(beta17);
   c18=cos(beta18);  s18=sin(beta18);
   c19=cos(beta19);  s19=sin(beta19);

    

end
XK1=[];
YK1=[];
XK2=[];
YK2=[];
XK3=[];
YK3=[];
XK4=[];
YK4=[];

X_pocatky=[0; 0];
Y_pocatky=[0; 0];
xk3=[0, 0,0,0,0];
yk3=[0, 0,0,0,0];
for it=1:size(QQ,2)
      
   beta4=QQ(1,it);
   beta7=QQ(2,it);
   
   b1=Z(1,it);
   b2=Z(2,it);

   beta3=Z(3,it);
   beta6=Z(4,it);
   beta10=Z(5,it);
   beta11=Z(6,it);
   beta12=Z(7,it);
   beta16=Z(8,it);
   beta5=beta3-delta;
   beta13=beta12-(pi/2);
   beta14=beta3-gama;
   beta15=beta10+alfa;
   beta19=beta4-pi;
   % mnohouhelnik 1. smycky
   % souradnice 1. vrcholu
   xk1=[X_pocatky(1)];
   yk1=[Y_pocatky(1)];
   xk1=[xk1,xk1(1)+b1*cos(beta1)];
   yk1=[yk1,yk1(1)+b1*sin(beta1)];
   xk1=[xk1,xk1(2)+b2*cos(beta2)];
   yk1=[yk1,yk1(2)+b2*sin(beta2)];
   xk1=[xk1,xk1(3)+b3*cos(beta3)];
   yk1=[yk1,yk1(3)+b3*sin(beta3)];
   xk1=[xk1,xk1(4)+b4*cos(beta4)];
   yk1=[yk1,yk1(4)+b4*sin(beta4)];
   XK1=[XK1; xk1];
   YK1=[YK1; yk1];

   % mnohouhelnik 2. smycky
   % souradnice 1. vrcholu
   xk2=[X_pocatky(2)];
   yk2=[Y_pocatky(2)];
   xk2=[xk2,xk2(1)+b1*cos(beta1)];
   yk2=[yk2,yk2(1)+b1*sin(beta1)];
   xk2=[xk2,xk2(2)+b2*cos(beta2)];
   yk2=[yk2,yk2(2)+b2*sin(beta2)];
   xk2=[xk2,xk2(3)+b3*cos(beta3)];
   yk2=[yk2,yk2(3)+b3*sin(beta3)];
   xk2=[xk2,xk2(4)+b5*cos(beta5)];
   yk2=[yk2,yk2(4)+b5*sin(beta5)];
   xk2=[xk2,xk2(5)+b6*cos(beta6)];
   yk2=[yk2,yk2(5)+b6*sin(beta6)];
   xk2=[xk2,xk2(6)+b7*cos(beta7)];
   yk2=[yk2,yk2(6)+b7*sin(beta7)];
   xk2=[xk2,xk2(7)+b8*cos(beta8)];
   yk2=[yk2,yk2(7)+b8*sin(beta8)];
   xk2=[xk2,xk2(8)+b9*cos(beta9)];
   yk2=[yk2,yk2(8)+b9*sin(beta9)];
   XK2=[XK2; xk2];
   YK2=[YK2; yk2];
   
   xk3=[b4*cos(beta4-pi)];
   yk3=[b4*sin(beta4-pi)];
   xk3=[xk3,xk3(1)+b10*cos(beta10)];
   yk3=[yk3,yk3(1)+b10*sin(beta10)];
   h1=xk3(2);
   v1=yk3(2);
   xk3=[xk3,xk3(2)+b11*cos(beta11)];
   yk3=[yk3,yk3(2)+b11*sin(beta11)];
   xk3=[xk3,xk3(3)+b12*cos(beta12)];
   yk3=[yk3,yk3(3)+b12*sin(beta12)];
   xk3=[xk3,xk3(4)+b13*cos(beta13)];
   yk3=[yk3,yk3(4)+b13*sin(beta13)];
   xk3=[xk3,xk3(5)+b14*cos(beta14)];
   yk3=[yk3,yk3(5)+b14*sin(beta14)];
   XK3=[XK3; xk3];
   YK3=[YK3; yk3];

   xk4=0;
   yk4=0;
   xk4=[xk4,xk4(1)+b4*cos(beta19)];
   yk4=[yk4,yk4(1)+b4*sin(beta19)];
   xk4=[xk4,xk4(2)+b15*cos(beta15)];
   yk4=[yk4,yk4(2)+b15*sin(beta15)];
   h2=xk4(3);
   v2=yk4(3);
   xk4=[xk4,xk4(3)+b16*cos(beta16)];
   yk4=[yk4,yk4(3)+b16*sin(beta16)];
   xk4=[xk4,xk4(4)+b17*cos(beta17)];
   yk4=[yk4,yk4(4)+b17*sin(beta17)];
   xk4=[xk4,xk4(5)+b18*cos(beta18)];
   yk4=[yk4,yk4(5)+b18*sin(beta18)];


   XK4=[XK4; xk4];
   YK4=[YK4; yk4];
   h1-h2; 
   v1-v2;
end

% Nastaveni rozmeru obrazku pro animaci .....................  
       
xmin=min([min(min(XK1)),min(min(XK2))]);            
xmax=max([max(max(XK1)),max(max(XK2))]);            
ymin=min([min(min(YK1)),min(min(YK2))]);            
ymax=max([max(max(YK1)),max(max(YK2))]);            
    
figure(7)                                                       
plot([xmin xmax],[ymin ymax],'k.')                              
axis equal                                                      
xymima=axis;                                                    
rozsahx=xymima(2)-xymima(1);                                    
rozsahy=xymima(4)-xymima(3);                                    
xymima=xymima+[-rozsahx/20 rozsahx/20 -rozsahx/33 rozsahx/33];  
% Animace pohybu
for it=1:nt
   xk1=XK1(it,:);
   yk1=YK1(it,:);
   xk2=XK2(it,:);
   yk2=YK2(it,:);
   xk3=XK3(it,:);
   yk3=YK3(it,:);
   xk4=XK4(it,:);
   yk4=YK4(it,:);
   li=plot(xk1,yk1,'b');set(li,'LineWidth',2.5); hold on 
   li=plot(xk2,yk2,'r');set(li,'LineWidth',1.5); hold on
   li=plot(xk3,yk3,'g');set(li,'LineWidth',1.5); hold on
   li=plot(xk4,yk4,'g');set(li,'LineWidth',1.5);

   title('Animace')
   xlabel('x  [m]')
   ylabel('y  [m]')
   axis(xymima);
   drawnow;
   % Zpomalovac animace  
   for kkk=1:50, kk=1+1; end
   hold off
end
