%% FREQUENZE PROPRIE - modello a elementi finiti
% FEM per frequenze proprie energy harvester
% Ricerca delle frequenze proprie e modi di vibrare di una trave
% vincolo come cantilever       

clear all
close all
clc

%% Calcolo lunghezza massima elemento finito

E1=24.0e9;%modulo di rigidezza [N/m2] 
rho1=3500; %densità [kg/m3]

b1=38.10/1000;
h1=0.61/1000;
L_clamp1=46.36/1000;

Area=b1*h1;
J1=b1*h1^3/12;

m1=rho1*Area %massa per u di lunghezza sez1 [kg/m]
EA1=E1*Area % [N]
EJ1=E1*J1 % [Nm2] 

fmax=2500; %freq massima dell'analisi

om=2*fmax*2*pi;
lmax=sqrt(pi^2/om*sqrt(EJ1/m1)) % [m]
%NB approssimare per difetto


%% Caricamento del file .inp e assemblaggio
[file_i,xy,nnod,sizee,idb,ngdl,incidenze,l,gamma,m,EA,EJ,posiz,nbeam,alfa,beta]=loadstructure

% Assemblaggio
[M,K]=assem(incidenze,l,m,EA,EJ,gamma);

% Plottaggio struttura indeformata
dis_stru(posiz,l,gamma,xy)
axis square


%% Massa concentrata

% massa concentrata (parallelepipedo)
Mc=15.6e-3; %[kg]
l_Mc=16e-3; %lunghezza massa [m];
s_Mc=1.5e-3; % spessore massa [m]
Jc=1/12*Mc*(l_Mc^2+4*s_Mc^2) ;%[kg]*[m^2]

n_nodo_massa=9;

in=3*(n_nodo_massa-2)+1;
fn=in+2;
M(in:fn,in:fn)=M(in:fn,in:fn)+[Mc 0 0; 0 Mc 0; 0 0 Jc];
    

%% Partizione
MLL=M(1:ngdl,1:ngdl);
KLL=K(1:ngdl,1:ngdl);
MLV=M(1:ngdl,ngdl+1:end);
KLV=K(1:ngdl,ngdl+1:end);
MVL=M(ngdl+1:end,1:ngdl);
KVL=K(ngdl+1:end,1:ngdl);
MVV=M(ngdl+1:end,ngdl+1:end);
KVV=K(ngdl+1:end,ngdl+1:end);

% Definizione della matrice di smorzamento
R=alfa*M+beta*K;

%partizione matrice di smorzamento
RLL=R(1:ngdl,1:ngdl);
RLV=R(1:ngdl,ngdl+1:end);
RVL=R(ngdl+1:end,1:ngdl);
RVV=R(ngdl+1:end,ngdl+1:end);


%% Analisi
% Calcolo di pulsazioni proprie e modi di vibrare

n_modi_dis=4; %numero di modi da disegnare

[modi,d]=eig(inv(MLL)*KLL);

fre=sqrt(diag(d))/2/pi;

fscala=1;
[Y, ind]=sort(fre);
modi_ind=ind(1:n_modi_dis);

tipoEF=ones(1,length(EA)); %vettore lungo num EF, val diverso da 0 se EF fittizio

for ii=1:n_modi_dis
    figure
disegG(modi(:,modi_ind(ii)),fscala,incidenze,l,gamma,posiz,tipoEF);
% diseg(modi(:,modi_ind(ii)),fscala,incidenze,l,gamma,posiz);
title(['f_',num2str(ii),'=',num2str(fre(modi_ind(ii))),' Hz'])
end

display(' ')
display('Frequenze proprie:');
for nn=1:n_modi_dis
    display(['Modo ', num2str(nn), ': ', num2str(fre(modi_ind(nn))), ' Hz']);
end


%% Calcolo reazione vincolare e risposta a spostamento di vincolo impresso
Xv=zeros(size(MVV,1),1);
Xv(2)=1;                                %spostamento di vincolo 1
vet_omega=0:.5*2*pi:1000*2*pi;   %risoluzione risposta in freq e fmax

for ii=1:length(vet_omega)
    om=vet_omega(ii);
    F0=-(-om^2*MLV+sqrt(-1)*om*RLV+KLV)*Xv;
    A=-om^2*MLL+sqrt(-1)*om*RLL+KLL;
    x(:,ii)=inv(A)*F0;
    Rv(:,ii)=(-om^2*MVL+sqrt(-1)*om*RVL+KVL)*x(:,ii)+(-om^2*MVV+sqrt(-1)*om*RVV+KVV)*Xv;
end

%plottaggio
figure
subplot(211)
plot(vet_omega/2/pi,abs(x(14,:)))           %gdl della risposta
hold on
ylabel('|X_A| [m/m]')
subplot(212)
plot(vet_omega/2/pi,angle(x(14,:))*180/pi)  %gdl della risposta
hold on
ylabel('\Psi [°]')
xlabel('[Hz]')

% figure
% subplot(211)
% plot(vet_omega/2/pi,abs(Rv(2,:)))           %gdl della reazione vincolare
% hold on
% ylabel('|RV_C| [N/m]')
% subplot(212)
% plot(vet_omega/2/pi,angle(Rv(2,:))*180/pi)  %gdl della reazione vincolare
% hold on
% ylabel('\Psi [°]')
% xlabel('[Hz]')