%Closed-Loop Responses from Open Loop Parameters
%Reference is special 3rd Order as in Section 10.A.  
%    Comments apply to that case in particular.
%  Dynamically choose r and gain multiplier
%  Modify program to add poles and zeros 
%  Null entry to causes retention of existing values 
 
r2deg = 180/pi; % for use in changing radians to degrees
% w -> Frequency points from 10^-1 to 10^1.  Modify to shift plots.
	w = logspace(-1,1,201);
% t -> Time points from 0 to 10.  Modify to shift plots.
	t = linspace(0,10,200); % Time points

r = 2; kmult = 1; typgr = 'b';
while (r >= 0)
	WL = 1; % default unity-gain frequency
% r is ratio of pole freq. to WL for nominal gain.
%  More generally it is half the ratio of added pole to added zero.
	r0 = input('Enter freq. ratio, r=(zero)/(2*pole)), negative to quit: ');
	if isempty(r0), % if user enters blank, old value retained
		disp(r);
	else 
		r = r0;
	end % if
	if (r >= 0),
		z = [-1/r]; % open loop zero is at WL/r
		p = [0;0;-r]; % open loop pole is at WL*r.  Also two poles at zero.
		kmult1 = input('Enter gain multiplier: '); 
				% ratio to K giving max phase margin
		if ~isempty(kmult1), kmult = kmult1; end;
		k = r*kmult % k = r for unity gain at geometric mean of pole, zero
		[nopen, dopen] = zp2tf(z,p,k); % open-loop num and den from poles, zeros, k
		den = nopen + dopen; % closed-loop denominator
		out = nopen; % numerator for output step response
		er = dopen; % numerator for error step response
		% integration of er by shifting right:
		rer = er*[0 1 0 0 ; 0 0 1 0 ; 0 0 0 1 ; 0 0 0 0]; 

		disp('');
		disp('b=Bode plot (open loop)');
		disp('o=Step Output');
		disp('e=Step Error');
		disp('r=Ramp Error');
		disp('f=Frequency Response');
		typgr1 = input('Enter b, o, e, r, or f. ', 's');
		if ~isempty(typgr1), typgr = typgr1; end;
		clg;
		if strcmp(typgr,'b')
			bode(nopen,dopen); % open-loop transfer function
			title('Bode Plot (Open-Loop)')
 	elseif strcmp(typgr,'e')
			step(er,den,t); % error response to a step
			title('Error Resposne to Step')
			grid;
		elseif strcmp(typgr,'o')
			step(out,den,t); % output response to a step
			title('Output Response to Step')
			grid;
		elseif strcmp(typgr,'r')
			step(rer,den,t); % error response to a ramp
			title('Error Response to Ramp')
			grid;
		else
 		hout = freqs(out, den, w); % frequency response to output
 		magout =  20*log10(abs(hout)); % magnitude of hout
 		phout = r2deg*angle(hout); % phase of hout
 		her = freqs(er, den, w); % frequency response to error
 		mager = 20*log10(abs(her)); % magnitude of her
 		pher = r2deg*angle(her); % phase of her
			subplot(211), semilogx(w, magout, 'r', w, mager, '--r')
 		grid
 		title('Frequency Response Magnitude.  solid = output   dash = error')
 		xlabel('Omega / Omega-n')
 		ylabel('dB')
 		subplot(212), semilogx(w, phout, 'g', w, pher, '--g')
 		grid
 	 title('Frequency Response Phase.  solid = output   dash = error')
 		xlabel('Omega / Omega-n')
 		ylabel('degrees')
		end % if typgr
	end % if r
end % while
