%TIME RESPONSE & PHASE PLANE PLOT, Hz UNITS
%Develop output time response and 
%  linear phase plane by ordinary matrix procedure

%some constants
      d2r = pi/180;
					r2d = 180/pi;
					r2H = 1/(2*pi);

% MODIFY PARAMETERS BELOW & GO
%*****************************
SR = 1 ; % 1 for step response, otherwise start at inital condition
Degrees_init = 57 ; % degrees initial phase
Finit = .159 ; % Hz initial frequency
D = 57 % step amplitude, degrees of phase
Fn = .159 % Hz' % natural frequency
z = .707 % damping factor
alpha = .5
ending = 10/Wn; % Student Matlab, v 3.5, limits ending/inc to 1024 max
inc = .05/Wn %sampling period
%******************************
     A = d2r*D;
     Wn = 2*pi*Fn;

%Generate vector of computation times
last = 1 + ending/inc; % index of last data points
t = linspace(0,ending,last); % linear sequence of times
dt = t(2)-t(1); % time increment

u = A*ones(1,last); % step input, amplitude A

%Polynominals for closed-loop transfer function:
		N = [     0   2*z*Wn*alpha  Wn^2   % numerator for phase
							2*z*Wn*alpha  Wn^2      0];   % numerator for frequency
		D = [     1       2*z*Wn    Wn^2]; % denominator
[a,b,c,d] = tf2ss(N,D);              % elements of dynamic matrix equation

if (SR == 1)&(A~=0),
	x0 = [0,0]; % step response starts with zero state variables;
 disp('STEP RESPONSE');
else % state variables to give required initial conditions
	Degrees_init
	Phinit = d2r*Degrees_init;
	Finit
	Winit = 2*pi*Finit;
	denom = N(1,2)^2*D(3)-N(1,3)*N(1,2)*D(2)+N(1,3)^2*D(1);
 x0(1)=(Winit*D(1)*N(1,3)+Phinit*N(1,2)*D(3)-u(1)*N(1,2)*N(1,3))/denom;
 x0(2)=(Phinit*(N(1,3)*D(1)-N(1,2)*D(2))-Winit*N(1,2)*D(1)+u(1)*N(1,2)^2)/denom;
 disp('START AT INITIAL CONDITIONS');
end % if

[Phi, Gamma] = C2D(a,b,dt); % values for use in solution eqution
x = ltitr(Phi,Gamma,u',x0); % solutions for each value of t
y = c*x'+d*u;

clf; % clear previous graphs
er = u - y(1,:);
subplot(211), plot(t,r2d*er,'g') 
grid
title('Time Response, Error (linear)')
xlabel('seconds')
ylabel('degrees')
subplot(212), plot(r2d*y(1,:),r2H*y(2,:),'r')
grid
title('Phase Plane, Output (linear)')
xlabel('phase (degrees)')
ylabel('freq (Hz)')
		
		

