% Matlab example: First order response to step, impulse, and ramp
% functions. 

clc
clf
clear all
close all

% Only one parameter is needed to define the response of a first order
% system, and it is the time constant tau. 

% take user input for a time constant
disp('Keep in mind that first order systems "reach" yfinal after 5 time constants')
T = input('Enter a time constant for the first order response (s): ');

% set the time parameter. 10 time constants should show the entire response
t = [0:.001:10*T];
% divide the time parameter by the time constant to establish
% nondimensional time
t_T = t/T;

%% Step function

% 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: ');

% use first order step response to find y. Start with the normalized y,
% which is simply the solution to the first order system.
y_step_norm =  (1 - exp(-t_T));

% multiply by the step height and add the original y value to get the
% actual response
y_step = (y_f-y_i)*y_step_norm + y_i;

% plot regular and normalized responses. 
figure (1)
hold on
subplot(1,2,1)
plot(t, y_step)
title('First order step response'), xlabel('time (s)'), ylabel('y')

hold on
subplot(1,2,2)
plot(t_T,y_step_norm)
title('First order normalized step response'), xlabel('time (nondimensional)'), ylabel('normalized y')


%% Ramp function
disp(' '),disp('Ramp function with same T'),disp(' ')

% the ramp response is the integral of the step response. The ramp function
% is likewise the integral of the step function.

% for a ramp function, take user input on intial y and slope A. 
y_i_ramp = input('Enter the intial value of y: ');
A = input('Enter the slope of the ramp function: ');

% determine actual ramp function. 
y_ideal_ramp = A*t + y_i_ramp;

% determine normalized response. This is simply the solution to the first
% order ramp response. 
y_ramp_norm = (t - T*(1-exp(-t_T)));
% determine actual response. Multiply by slope and add y_i_ramp to get actual response.
y_ramp = A*y_ramp_norm + y_i_ramp;

% plot actual response of y and the ramp function. 
figure (2)
subplot(1,2,1)
hold on
plot(t, y_ideal_ramp, 'k')
plot(t, y_ramp, 'r')
title('First order ramp response'), xlabel('time (s)'), ylabel('y')

% plot normalized ramp response
figure (2)
subplot(1,2,2)
hold on
plot(t_T, y_ramp_norm)
plot(t_T, t, 'k')
title('First order normalized ramp response'), xlabel('time (nondimensional)'), ylabel('normalized y')


% compute error and time lag, and compare to expected values. 
% take limit of ideal function minus measured function as both approach
% infinity. 

syms tt y % use tt for time since t will be used again later.
% create symbolic function for difference between ideal and measured
% responses. The difference is the error.
y_ideal_symb = (A*tt + y_i_ramp); 
y_ramp_symb = (A*(tt - T*(1-exp(-tt/T)))+y_i_ramp);
error_diff = y_ideal_symb - y_ramp_symb;
error1 = limit(error_diff,tt,Inf);
% compare to error after 5 time constants.
i = floor(5*T/.001); % index of ideal y and response at 5 time constants. 
% floor function prevents error from non-integer indices. 
error2 = y_ideal_ramp(i) - y_ramp(i);
% compare to expected error = A*T
error3 = A*T;

% output using fprintf
fprintf('\n')
fprintf('Error as t goes to infinity: %.3f\n', double(error1)) 
fprintf('                Error at 5T: %.3f\n', error2)
fprintf('         Expected error A*T: %.3f\n', error3)

% to compute time lag, solve the ideal and ramp equations for time using
% the built in Matlab equation solver. 
ideal_time = solve(y_ideal_symb == y,tt);
ideal_ramp = solve(y_ramp_symb == y,tt);
lag_diff = ideal_ramp - ideal_time;
% Take the limit of the difference as y goes to infinity. 
lag1 = limit(lag_diff,y,Inf);

% lag at five time constants.
% first find value of y_ramp at t = 5*T. Use index i from earlier.
y_5T = y_ramp(i);
% time here is simply t = 5*T. 
% find time when ideal function = y_5T. This equation is easily solved by
% hand. 
t_ideal = (y_5T - y_i_ramp)/A;
% lag at five time constants is the difference between 5*T and t_ideal
lag2 = 5*T - t_ideal;

% expected lag is simply T
lag3 = T;

% output using fprintf
fprintf('\n')
fprintf('               Computed lag: %.3f\n', double(error1)) 
fprintf('                  Lag at 5T: %.3f\n', error2)
fprintf('             Expected lag T: %.3f\n', error3)


%% impulse function
disp(' '),disp('Impulse function with same T'),disp(' ')

% The impulse response is simply the derivative of the step response, since
% the impulse is like the derivative of the step function.

% for an impulse function, take user input on intial y and impulse y. 
y_i_impulse = input('Enter the intial value of y: ');
y_f_impulse = input('Enter the impulse value of y: ');

y_impulse_norm = (1/T)*exp(-t_T);
y_impulse = y_impulse_norm * (y_f_impulse-y_i_impulse) + y_i_impulse;

figure(3)
subplot(1,2,1)
hold on
plot(t_T, y_impulse_norm)
title('First order normalized impulse response'), xlabel('time (s)'), ylabel('normalized y')

figure(3)
subplot(1,2,2)
hold on
plot(t,y_impulse)
plot([0 0],[y_i_impulse,y_f_impulse],'k')
xlim([-T,10*T])
title('First order impulse response'), xlabel('time (s)'), ylabel('y')




