% Kmitanie kyvadla pristupom
% Recursive - to znamena riesenie ODE -> phi(t)
clear all
clc

% Geometricke a hmotnostne charakteristiky
Ax = 0.2;   % [m] Poloha bodu A
Ay = 0.3;   % [m] Poloha bodu A
R = 0.005;  % [m] Polomer tyce
L = 0.5;    % [m] Dlzka tyce
ro = 7801;  % [kg/m^3]hustora
g = 9.80665;% [m/s^2]

% Vypocet hmotnostnych charakteristik
m = ro*L*pi*R^2;       % [kg] Hmotnost kyvadla
IT = (1/12)*m*L^2;     % [kg*m^2] Moment zotrvacnosti kyvadla k tazisku
IA = IT + m*(L/2)^2;   % [kg*m^2] Moment zotrvacnosti kyvadla k bodu A

% Casovy vektor
dt  = 0.01;
tend = 2;
time = 0:dt:tend;

% Modalna analyza
f0 = sqrt(1.5*g/L)/2/pi;    % Linearizovana hodnota
Tper = 1/f0;

% IC
phi0 = 0*pi/180;    % Pociatocne natocenie mearne od osi x GSS
w0 = 0*pi/180;
IC = [phi0 w0];      

% Riesenie ODE
par = [IA,m,g,L];
[t,Y] = ode45(@FCN_kyvadlo,time,IC,[],par);

% Polohy
phi = Y(:,1);
xT = Ax + (L/2)*cos(phi);
yT = Ay + (L/2)*sin(phi);

xL = Ax + L*cos(phi);
yL = Ay + L*sin(phi);

% Rychlosti
w = Y(:,2);
vxT = -w.*(L/2).*sin(phi);
vyT = w.*(L/2).*cos(phi);

% Zrychlenia
alpha = -(m*g*(L/2)/IA)*cos(phi);
axT = -alpha.*(L/2).*sin(phi) - w.^2.*(L/2).*cos(phi);
ayT = alpha.*(L/2).*cos(phi) - w.^2.*(L/2).*sin(phi);

% Reakcie (z d'Alembertovho principu)
Rx = m*axT;
Ry = m*g + m*ayT;

% Uhlove veliciny
figure(1), clf
    subplot(3,1,1)
plot(t,phi*180/pi,'r','Linewidth',2), hold on
xlabel('t [s]'), ylabel('\phi, [deg]'), title('Natocenie kyvadla')
grid on
    subplot(3,1,2)
plot(t,w*180/pi,'r','Linewidth',2), hold on
xlabel('t [s]'), ylabel('\omega [deg/s]'), title('Uhlova rychlost kyvadla')
grid on
    subplot(3,1,3)
plot(t,alpha*180/pi,'r','Linewidth',2), hold on
xlabel('t [s]'), ylabel('\alpha [deg/s^2]'), title('Uhlove zrychlenie kyvadla')
grid on

% Tazisko
figure(2), clf
    subplot(3,1,1)
plot(t,xT,'r','Linewidth',2), hold on
plot(t,yT,'--','Linewidth',2)
xlabel('t [s]'), ylabel('xT, yT [m]'), title('Poloha taziska v osi x a y')
legend('xT','yT'), grid on
    subplot(3,1,2)
plot(t,vxT,'r','Linewidth',2), hold on
plot(t,vyT,'--','Linewidth',2)
xlabel('t [s]'), ylabel('v_{xT}, v_{yT} [m/s]'), title('Rychlost taziska v osi x a y')
legend('v_{xT}','v_{yT}'), grid on
    subplot(3,1,3)
plot(t,axT,'r','Linewidth',2), hold on
plot(t,ayT,'--','Linewidth',2)
xlabel('t [s]'), ylabel('a_{xT}, a_{yT} [m/s^2]'), title('Zrychelenie taziska v osi x a y')
legend('a_{xT}',' a_{yT}'), grid on

% Rekacie v bode A
figure(3), clf
plot(t,Rx,'r','Linewidth',2), hold on
plot(t,Ry,'--','Linewidth',2)
xlabel('t [s]'), ylabel('R_x, R_y [N]'), title('Reakcie v bode A')
legend('R_x','R_y'), grid on

% Animacia pohybu kyvadla
for i = 1:length(xT)
    figure(5), clf
    plot(Ax,Ay,'ok','MarkerSize',15,'LineWidth',3), hold on
    plot([Ax,xL(i)],[Ay,yL(i)],'.-r','Linewidth',10,'MarkerSize',30), hold on
    plot(xT(i),yT(i),'.k','MarkerSize',15), hold on
    axis square
    axis([Ax-0.6 (Ax+0.6) Ay-0.6 (Ay+0.6)])
    title('Animacia pohybu kyvadla')
    grid on
    
    pause(0.01)
end

% FFT
N = length(time);        % No. of samples
NFFT = 2^nextpow2(N);    % Next power of 2 from length of phi
PHI = fft(phi,NFFT)/N;   % FFT
f = (1/dt)/2*linspace(0,1,NFFT/2+1); % Frekvencny vektor

figure(6), clf
plot(f,2*abs(PHI(1:NFFT/2+1)),'LineWidth',2)
title('Jednostranne amplitudove spektrum \phi(t)')
xlabel('Frekvencia (Hz)')
ylabel('|\Phi(f)|')
