% Vektorová metóda kinematickej analýzy VMS
% Riesenie polohy, rychlostia zrychleni clenov rovinneho R-R-R-R mechanizmu 
clear all
clc

% Parametre
h1 = 40;
h2 = 150;
h3 = 200;
h4 = 260;
alpha = pi;

% Uhlova rychlost vstupneho clena 
w = 2*pi;  % [rad/s]

% Casovy rozsah
dt = 0.01;          %[s] casova oblast
t_end = 1;          %[s] casovy krok
time = 0:dt:t_end;  %[s] casovy vektor
t_steps = length(time);

%% Vypocet polohy
% Vypocet sluckovych rovnic pre zadany casovy rozsah

% Startovacie hodnoty pre N-R metodu
psiZ1_0 = 80*pi/180;
psiZ2_0 = 290*pi/180;
z0 = [psiZ1_0 psiZ2_0];

% Alokacia premennych
psiN1 = zeros(1,t_steps);
psiZ1 = zeros(1,t_steps);
psiZ2 = zeros(1,t_steps);

i = 1;
for t = time
    psiN1(i) = w*t;
    par = [h1 h2 h3 h4 alpha psiN1(i)];
    z = fsolve(@FCN_mech_fsolve,z0,[],par);
    psiZ1(i) = z(1);
    psiZ2(i) = z(2);
    i = i + 1;
    z0 = [z(1) z(2)];
end

figure(1), clf
subplot(3,1,1)
plot(time,psiN1*180/pi,'Linewidth',2)
title('Polohy clenov mechanizmu')
xlabel('t [s]'), ylabel('\psi_{N1} [deg]')
grid on
subplot(3,1,2)
plot(time,psiZ1*180/pi,'k','Linewidth',2)
xlabel('t [s]'), ylabel('\psi_{Z1} [deg]')
grid on
subplot(3,1,3)
plot(time,psiZ2*180/pi,'r','Linewidth',2)
xlabel('t [s]'), ylabel('\psi_{Z2} [deg]')
grid on

%% Vypocet rychlosti
dpsiN1 = zeros(1,t_steps);
dpsiZ = zeros(2,t_steps);

i = 1;
for t = time
    psiN1(i) = w*t;
    dpsiN1(i) = w;
    Jz = [-h2*sin(psiZ1(i)) -h3*sin(psiZ2(i));
           h2*cos(psiZ1(i))  h3*cos(psiZ2(i))];
    Jn = [-h1*sin(psiN1(i));
           h1*cos(psiN1(i))];
    
    dpsiZ(:,i) = inv(Jz)*(-Jn*dpsiN1(i));
    
    i = i + 1;
end

dpsiZ1 = dpsiZ(1,:);
dpsiZ2 = dpsiZ(2,:);

figure(2), clf
subplot(3,1,1)
plot(time,dpsiN1*180/pi,'Linewidth',2)
title('Rychlosti clenov mechanizmu')
xlabel('t [s]'), ylabel('d\psi_{N1} [deg/s]')
grid on
subplot(3,1,2)
plot(time,dpsiZ1*180/pi,'k','Linewidth',2)
xlabel('t [s]'), ylabel('d\psi_{Z1} [deg/s]')
grid on
subplot(3,1,3)
plot(time,dpsiZ2*180/pi,'r','Linewidth',2)
xlabel('t [s]'), ylabel('d\psi_{Z2} [deg/s]')
grid on

%% Vypocet zrychleni
ddpsiN1 = zeros(1,t_steps);
ddpsiZ = zeros(2,t_steps);

i = 1;
for t = time
    ddpsiN1(i) = 0;
    Jz = [-h2*sin(psiZ1(i)) -h3*sin(psiZ2(i));
           h2*cos(psiZ1(i))  h3*cos(psiZ2(i))];
    dJz = [-dpsiZ1(i)*h2*cos(psiZ1(i)) -dpsiZ2(i)*h3*cos(psiZ2(i));
           -dpsiZ1(i)*h2*sin(psiZ1(i)) -dpsiZ2(i)*h3*sin(psiZ2(i))];
    Jn = [-h1*sin(psiN1(i));
           h1*cos(psiN1(i))];
    dJn = [-dpsiN1(i)*h1*cos(psiN1(i));
           -dpsiN1(i)*h1*sin(psiN1(i))];
       
    ddpsiZ(:,i) = inv(Jz)*(-dJz*dpsiZ(:,i) - Jn*ddpsiN1(i) - dJn*dpsiN1(i));
    
    i = i + 1;
end

ddpsiZ1 = ddpsiZ(1,:);
ddpsiZ2 = ddpsiZ(2,:);

figure(3), clf
subplot(3,1,1)
plot(time,ddpsiN1*180/pi,'Linewidth',2)
title('Zrychlenia clenov mechanizmu')
xlabel('t [s]'), ylabel('dd\psi_{N1} [deg/s^2]')
grid on
subplot(3,1,2)
plot(time,ddpsiZ1*180/pi,'k','Linewidth',2)
xlabel('t [s]'), ylabel('dd\psi_{Z1} [deg/s^2]')
grid on
subplot(3,1,3)
plot(time,ddpsiZ2*180/pi,'r','Linewidth',2)
xlabel('t [s]'), ylabel('dd\psi_{Z2} [deg/s^2]')
grid on

%% Animacia pohybu klukoveho mechanizmu
i = 1;
for t = time
    Ax = 0;
    Ay = 0;
    Bx = h1*cos(psiN1(i));
    By = h1*sin(psiN1(i));
    Cx = h4 - h3*cos(-psiZ2(i));
    Cy = h3*sin(-psiZ2(i));
    Dx = h4;
    Dy = 0;
 
    figure(4),clf
    plot([Ax Bx],[Ay By],'-o','Linewidth',8,'Markersize',8), hold on
    plot([Bx Cx],[By Cy],'-o k','Linewidth',8,'Markersize',8), hold on
    plot([Cx Dx],[Cy Dy],'-o r','Linewidth',8,'Markersize',8), hold on
    title('Animacia pohybu mechanizmu')
    grid on
    axis([-100 300 -200 200])
    axis square
    pause(0.1)
    
    i = i + 1;
end