clear
close all
% Materiaux: acier
  pois = 0.3;        rho_st = 7800.;
  E_st = 210000e6;   G_st = E_st / (2*(1+pois));

% Géometrie
  L = 1;       % longueur de la poutre
  
  Mu = 1;    % masse au bout de la poutre
  Iu = 1;    % inertie de la masse au bout de la poutre
  
% Proprietés des éléments
  A = 7.58e-4; 
  Iy = 6.29e-8;  Iz = 77.8e-8;  J = Iy+Iz;
  Ep_poutre = [E_st  G_st  A Iy Iz J A*rho_st];
  
% geometrie du modèle
  
  ne = 5;
  Coord (1,:) = [0  0  0];
  for i = 1:ne
    Coord (1+i,:) = [ Coord(i,1)+L/ne  0  0 ];
    Elem(i,:) = [ i  i+1 ];
  end
  ePoutre = [1:ne];
  n_fix = 1;
  n_free = ne+1;
   
% Topologie du modèle
  [n_nodes,n_dof,n_elem,n_nel,Dof,Edof] = topol (Coord,Elem);
  [Ex,Ey,Ez] = coordxtr(Edof,Coord,Dof,n_nel); 
  
% Plot modèle
  Coord_xy(:,1) = Coord(:,1);	
  Coord_xy(:,2) = Coord(:,2);
  figure; femdraw2 (Coord_xy,Ex,Ey); 
  ylabel('y'); grid;
  
  break
  
%  Matrices K et M 
  nd = n_nodes*n_dof;
  K = zeros(nd); M = zeros(nd); C = zeros(nd); 
  
  ie = 1;
  eo(ie,:) = [0 0 1];
  [ke,me] = beam3d (Ex(ie,:),Ey(ie,:),Ez(ie,:),eo(ie,:),Ep_poutre);

  K = assem(Edof(ie,:),K,ke);
  M = assem(Edof(ie,:),M,me);
  
% Masse au bout de la poutre  
for i = 1:3
    ndof = (n_free-1)*n_dof+i;
    M(ndof,ndof) = M(ndof,ndof) + Mu;
end
for i = 4:6
    ndof = (n_free-1)*n_dof+i;
    M(ndof,ndof) = M(ndof,ndof) + Iu;
end


% Modes et vecteurs propres
% on obtient:  n_modes = nombre de modes calculés
%              freq = fréquences propres (Hz)
%              Egv = vecteurs propres (eigenvectors)

  b = [1:6];   % on fixe le noeud 1: Dof 1 à 6
  [L,Egv] = eigen (K,M,b);
  freq = sqrt(L)/(2*pi)  % fréquence propre en Hz
  n_modes = length (freq)
  
  for i = 1:length(freq)
    forme_t = reshape (Egv(:,i),n_dof,n_nodes); % extraction de chaque mode
    forme = forme_t';
    forme_free(i,:) = forme(n_free,:);
  end
  forme_free    % afficher les formes modales
  
  % Un input (actionneur) -----------------------------------
  in = zeros(n_nodes*n_dof, 1);
  in(8) = 1;   % DoF dir Y de n_free
  inm = Egv'*in;                           % forces modales

% Un output ------------------------------------------------
  out = zeros(1, n_nodes*n_dof);
  out(8) = 1;   % DoF dir Y de n_free
  outm  = [ out*Egv   zeros(1,n_modes) ];  % déplacements modaux

  freqvec = logspace(0,3,100)'; % de 0 à 1000 Hz selon une échelle logarithmique
  w=2*pi*freqvec;               % vecteur de pulsations en rad/s
  om = 2*pi*freq;           % nos fréquences propres en rad/s
  sda = 0.002;              % amortissement structurel = 1/2*Q

  xf = nor2xf (om,sda,inm,outm,w);
  figure; loglog (freqvec,abs(xf)); grid 
  title ('Réponse en fréquence pour F = 1 N'); xlabel('Hz')

  [a,b,c,d] = nor2ss (om,sda,inm,outm);  % modèle state space
  sys = ss(a,b,c,d);
  size_of_sys = size(sys)
  my_plot_bode (w, sys(1,1),'b','Réponse à l''extrêmité');

% Calcul statique
% Conditions aux limites
bc = [];  [b,bc,nb] = fix_point (bc, n_fix, Dof);

%  On définit les forces et moments
p = zeros(size(K,1),1);
i = n_free;  dof_exc = (i-1)*n_dof+2;
p(dof_exc) = 10;   % N

[X, R, xyzF] = fe_stat (K,p,b,n_dof,n_nodes);

Edb = extract (Edof,X);	
figure; femdraw2 ([Coord(:,1) Coord(:,2)],Ex,Ey); 
Edbxy = [Edb(:,1) Edb(:,2) Edb(:,6) Edb(:,7) Edb(:,8) Edb(:,12)];
femdisp2 (Ex,Ey,Edbxy);  ylabel('y');
title('Calcul statique - force 1 N selon Y au noeud 2');

disp ('Déplacements des noeuds (mm)');
disp (xyzF*1000)

% gain proportionnel du PID
Kp = 20e6