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

% MODIFY PARAMETERS BELOW & GO
%*****************************
SR = 1 ; % 1 for step response, otherwise start at inital conditions
Phinit = 1 ; % radian initial phase
Winit = 1 ; % radian/sec initial frequency
A = 1 % radian phase step amplitude
Wn = 1 % rad/sec' % natural frequency
z = .707 % damping factor
alpha = .5
ending = 10/Wn; % Student Matlab limits ending/inc to 1024 max
inc = .05/Wn %sampling period
%******************************

% 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
	Phinit
	Winit
	denom = N(1,2)^2*D(3)-N(1,3)*N(1,2)*D(2)+N(1,3)^2;
 x0(1)=(Winit*N(1,3)+Phinit*N(1,2)*D(3)-u(1)*N(1,2)*N(1,3))/denom;
 x0(2)=(Phinit*(N(1,3)-N(1,2)*D(2))-Winit*N(1,2)+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 eqaution
x = ltitr(Phi,Gamma,u',x0); % solutions for each value of t
y = c*x'+d*u; % outputs

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

