% Matlab example: Displaying second order dynamic response to step function
% with two steps. 

clc
clear all
close all

% Two parameters needed to define the dynamic response of a second order system
% Natural frequency wn (omega n)
% Damping ratio d (xi)
% It is easier for the user to define the natural frequency by defining the period of 
% oscillations in seconds.

% take user input for the period of observed oscillations
T_d = input('Enter the period of observed oscillations (s): ');

% from T_d, find damped natural frequency f_d (Hz) and damped natural frequency w_d (rad/s)
f_d = 1/T_d;
w_d = 2*pi*f_d;

% next, take user input for underdamped and overdamped natural frequencies. 
d_under  = input('Enter a damping ratio between 0 and 1 for an underdamped system: ');
d_over = input('Enter a damping ratio greater than 1 for the overdamped system: ');


% find undamped natural frequency and phase shift phi for the underdamped system.
f_n = f_d * sqrt(1-d_under^2);
w_n = 2*pi*f_d;
phi = asin(sqrt(1-d_under^2));

% for a step function, take user input on final and initial values.
y_i = input('Enter the initial value of y: ');
y_f = input('Enter the final value of y: ');

% define time. 10 periods seems like a good amount of time. 
t = [0:.001:10*T_d];
% natural frequency * time is a good time step for normalized y.
w_nt = w_n.*t;

% Want to display zero damping case, underdamped, critically damped, and overdamped. 

% zero damping, d = 0
d_zero = 0;
phi_zero = asin(sqrt(1-d_zero^2));
y_norm_zero = 1 - (exp(-d_zero*w_n.*t).*((1/sqrt(1-d_zero^2))*sin(w_n.*t.*sqrt(1-d_zero^2)+phi_zero)));
y_zero = y_norm_zero * (y_f-y_i) + y_i;

% user defined underdamped 
y_norm_under = 1 - (exp(-d_under*w_n.*t).*((1/sqrt(1-d_under^2))*sin(w_n.*t.*sqrt(1-d_under^2)+phi)));
y_under = y_norm_under * (y_f-y_i) + y_i;

% critical damping, d = 1
d_crit = 1;
y_norm_crit = 1 - (exp(-w_n.*t).*(1+w_n.*t));
y_crit = y_norm_crit *(y_f-y_i) + y_i;

% user defined overdamped
y_norm_over = 1 - (exp(-d_over*w_n.*t).*(cosh(w_n.*t*sqrt(d_over^2 - 1)) + (d_over/sqrt(d_over^2 - 1))*sinh(w_n.*t*sqrt(d_over^2 - 1))));
y_over = y_norm_over *(y_f-y_i) + y_i;

% plot all four damping cases with y and y normalized.
% first, regular y. 
figure(1)
hold on
plot(t, y_zero, 'b')
plot(t, y_under, 'g')
plot(t, y_crit, 'r')
plot(t, y_over, 'k')
% title, axis labels, and legend
title('Second order step response')
xlabel('time (s)')
ylabel('response y')
legend('Zero Damping','Underdamped','Critically Damped','Overdamped','Location','SouthOutside','Orientation','horizontal')


% now plot the normalized version
figure(2)
hold on
plot(w_nt, y_norm_zero, 'b')
plot(w_nt, y_norm_under, 'g')
plot(w_nt, y_norm_crit, 'r')
plot(w_nt, y_norm_over, 'k')
% title, axis labels, and legend
title('Normalized second order step response')
xlabel('nondimensional time, w_n*t')
ylabel('normalized y')
xlim([0, w_n*10*T_d])
legend('Zero Damping','Underdamped','Critically Damped','Overdamped','Location','SouthOutside','Orientation','horizontal')


% What does the response look like if there is another step before this one
% settles?

% take user input for the magnitude of a second step and the time at which
% it occurs.


t_2 = input('Enter the time in seconds the second step occurs: ');
y_f2 = input('Enter the magnitude of the second step: ');

% define interval. 
n = (t_2/.001);
t2 = [0:.001:(10*T_d - t_2)];
y_norm_under2 = 1 - (exp(-d_under*w_n.*t2).*((1/sqrt(1-d_under^2))*sin(w_n.*t2.*sqrt(1-d_under^2)+phi)));

% define the response from the second step beginning at t = t_2.
% Response at t < t_2 = 0. 
y_under2 = [zeros(1,n), y_norm_under2 * (y_f2-y_f)];
y_norm_crit2 = 1 - (exp(-w_n.*t2).*(1+w_n.*t2));
y_crit2 = [zeros(1,n), y_norm_crit2 *(y_f2-y_f)];
y_norm_over2 = 1 - (exp(-d_over*w_n.*t2).*(cosh(w_n.*t2*sqrt(d_over^2 - 1)) + (d_over/sqrt(d_over^2 - 1))*sinh(w_n.*t2*sqrt(d_over^2 - 1))));
y_over2 = [zeros(1,n), y_norm_over2 *(y_f2-y_f)];

% to superimpose, simply add the two functions for each damping constant. 
y_under_super = y_under + y_under2;
y_crit_super = y_crit + y_crit2;
y_over_super = y_over + y_over2;

% plot the superimposed responses
figure(3)
hold on
plot(t, y_under_super, 'b')
plot(t, y_crit_super, 'g')
plot(t, y_over_super, 'r')
% title, axis labels,
title('Second order step response with second step')
xlabel('time (s)')
ylabel('normalized y')
% include a plot of the actual steps and a legend
plot([0,0,t_2, t_2, 10*T_d],[0,y_f,y_f,y_f2, y_f2],'k')
legend('Underdamped','Critically Damped','Overdamped','Step Function','Location','SouthOutside','Orientation','horizontal')




