% Coupled electrmechanical dynamics  of MEMS structures
% using linear normal mode summation method and
% parallel plate approximation method
% Written for MEAM 550 in Fall 1998
% -- Suresh G.K.  Ananthasuresh

%
% This script needs the following m-files
% fem2.m  matcut.m veccut.m stfew.m
%

clear all
hold off
clc
clf
axis('normal');


global GK GM evec L B ic voltage v_time FM Flag freq ...
vdc vac omega gap e_force_elems ncnt

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Specify the following
%
length = 320;  % Length of the beam  in microns
NELEM = 40;   % Number of elements
% Property value specification
H = 1.5*ones(NELEM,1); % Thicknesses of beam elements in microns
B = 25*ones(NELEM,1); % Widths of the beam elements
% NOTE: This is in plane vibration.
%
E = 155.0e3;    % Units: microN/micron^2
rho = 2300e-18; % Units: microKg/micron^3

vdc  = input('Enter voltage:');
vac = 0.0;
omega = 1e6*2*pi;
gap = 1.0;

% Elements on which electrostatic force is applied.
e_force_elems = [3:NELEM-2];

% Number of cycles to run the simulation
% This is based on first natural frequency
Ncycles = 2;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

NNODE = NELEM  + 1;
nx = [0:length/NELEM:length];
ny = zeros(size(nx));

for ielem=1:NELEM,
    ncnt(ielem,1) = ielem;
    ncnt(ielem,2) = ielem+1;
end


%
% Displacement boundary condition specification
% Fixed-fixed beam
dispID = [1 2 (NNODE-1)*2+1 (NNODE-1)*2+2];
dispVal = [0 0 0 0];

% Lengths of elements
for ie=1:NELEM,
  eye = ncnt(ie,1);
  jay = ncnt(ie,2);
  L(ie) = sqrt ( (nx(jay) - nx(eye))^2 + (ny(jay) - ny(eye))^2 );
end


%
% Call the fem m-file
% K = stiffness matrix
% M = inertia (mass) matrix
%
[K, M] = fem2(B,H,L,E,rho,nx,ny,ncnt,NELEM,NNODE,dispVal,dispID);

% Compute mode shapes and natural frquencies
% evec = eigenvectorr matrix
% eval = diagonal matrix containing the eigenvalues
D = inv(K)*M;
[evec eval] = eig(D);

% Normalize  mode  shapes so that the maximum transverse
% displacement is unity.
sz  =  size(evec,2);
evec(:,1) = evec(:,1) / max(abs(evec(1:2:sz,1)));
evec(:,2) = evec(:,2) / max(abs(evec(1:2:sz,2)));
evec(:,3) = evec(:,3) / max(abs(evec(1:2:sz,3)));
evec(:,4) = evec(:,4) / max(abs(evec(1:2:sz,4)));

% Generalized inertia matrix
GM = evec'*M*evec;

% Generalized stiffness matrix
GK = evec'*K*evec;

% Natural frequencies
freq = sqrt(diag(GK)./diag(GM));

% Compare frequencies with the  formula estimate of frequenc
%rho_l = rho*B(1)*H(1); % mass per unit length
%omega1 = sqrt( (E*B(1)*H(1)^3/12) / (rho_l*length^4) ) * 22.4;
%omega2 = sqrt( (E*B(1)*H(1)^3/12) / (rho_l*length^4) ) * 61.7;
%omega3 = sqrt( (E*B(1)*H(1)^3/12) / (rho_l*length^4) ) * 121;

%echo  on
% Computed & formula estimate
%[freq(1) omega1; freq(2) omega2; freq(3) omega3;]
% percentage error in computation
%[(omega1-freq(1))/omega1*100; (omega2-freq(2))/omega2*100; ...
% (omega3-freq(3))/omega3*100]
%echo off

% plot mode shapes
figure(1)
hold off
clf
plot([0 nx(2:NNODE-1) length], [0 evec(1:2:sz,1)' 0],'-r', ...
     [0 nx(2:NNODE-1) length], [0 evec(1:2:sz,2)' 0],'--g', ...
     [0 nx(2:NNODE-1) length], [0 evec(1:2:sz,3)' 0],':b',  ...
     [0 nx(2:NNODE-1) length], [0 evec(1:2:sz,4)' 0],'-.m');
legend('Mode1', 'Mode2', 'Mode3', 'Mode4');  
hold on
plot([0 nx(2:NNODE-1) length], [0; evec(1:2:sz,1); 0],'.r', ...
     [0 nx(2:NNODE-1) length], [0; evec(1:2:sz,2); 0],'.g', ...
     [0 nx(2:NNODE-1) length], [0; evec(1:2:sz,3); 0],'.b',  ...
     [0 nx(2:NNODE-1) length], [0; evec(1:2:sz,4); 0],'.m');

grid
xlabel('Length along the beam (x)');
ylabel('Transverse displacement of the modes');

% Frequencies in Hertz
fourFreqs_inHz=[freq(1) freq(2) freq(3) freq(4)]/2/pi

T0 = 0.0;
Tfinal = Ncycles * 2*pi/freq(1);
Tspan = [T0:Tfinal/100:Tfinal];
options = odeset('RelTol',1e-4,'AbsTol',[1e-4 1e-4 1e-5]);

% Take only first few modes for simulation
FM = 4; %  Number  of modes considered

Y0 = zeros(1,2*FM);

ic = 0; % Index used to  keep track of voltage as a function  of time

    [T, Y] = ode23('stfew',Tspan, Y0);

% Plot deformed beam at different time instants
% Take FM modes
idvec = 1:2:2*FM;
x_soln = Y(:,idvec);
x_soln = evec(:,1:FM)*x_soln';

figure(2)
clf

pmax = size(T,2);
for ip=1:1:pmax,
   plot([0 nx(2:NELEM) length], [0; x_soln(1:2:size(K,2),ip); 0],'-y');
end

hold on
% Plot first four modal coordinates' variation with time
clf
subplot(2,2,1);
plot(T, Y(:,1)); ylabel('Mode1'); xlabel('Time (sec)');
subplot(2,2,2);
plot(T, Y(:,3)); ylabel('Mode2'); xlabel('Time (sec)');
subplot(2,2,3);
plot(T, Y(:,5)); ylabel('Mode3'); xlabel('Time (sec)');
subplot(2,2,4);
plot(T, Y(:,7)); ylabel('Mode4'); xlabel('Time (sec)'); 

% Plot disp variation with time of the mid node 
figure(3)
clf
mid = NELEM - 1;
plot(T, x_soln(mid,:),'-b'); hold on; plot(T, x_soln(mid,:),'.r')
grid;
xlabel('Time (sec)');
ylabel('Max. disp.')

% Mid node Velocity

x_dot = evec(:,1:FM)*Y(:,2:2:2*FM)';
figure(4)
clf
plot(T, x_dot(mid,:),'-b'); hold on; plot(T, x_dot(mid,:),'.r')
grid;
xlabel('Time (sec)');
ylabel('Vel. of mid point');

%  Plot the  voltage applied as a  function of time
figure(5)
clf
plot(v_time, voltage);
xlabel('Time (sec)');
ylabel('Voltage (V)');








