%Open and closed loop modulation responses
%        with 1 pair of sampling sidebands
% Input in Hz, plot vs. Hz, display 0 dB for two N values
% Type 1, Order 3 (filter: pole, zero, pole)
%   Eq. (12.1) in FS by PL
% Adds one pair of sidebands at ± sampling frequency
%   Eq. (7.26) with P=1 in FS by PL
% Choose presence of zero-order hold
% Additional comments are in BdT1o3.m

clear
% MODIFY PARAMETERS AND GO
ZH = 1; % 1 for zero-order hold present, 0 for not present
Fref = 10000; % reference (sampling) frequency
Kp = 3;
KLF = 3;
Kv = 2e8;
Fp1 = 1;
Fz = 300;
Fp2 = 3000;
N = 1000;
fstart = 1;
fstop = Fref/2; % plot to half sampling frequency
op = 1;
%********

fprintf('\nBode Plot for loop with:');
fprintf('\none pair of sidebands produced by sampling;');
fprintf('\nKp = %g V/cycle; KLF = %g; Kv = %g Hz/V; N = %g;',Kp,KLF,Kv,N);
fprintf('\nFref = %g Hz; Fp1 = %g Hz; Fz = %g Hz; Fp2 = %g Hz;',Fp1,Fz,Fp2);
if ZH
	fprintf('\nZero-order hold present.\n')
else
	fprintf('\nNo hold present.\n')
end % if

twopi = 2*pi;
deg2rad = pi/180; % conversion constant, degrees to radians
%% fstop = Fref/2;
ws = twopi*Fref; % sampling frequency in rad/sec
Wp1 = twopi*Fp1;
Wp2 = twopi*Fp2;
Wz = twopi*Fz;
K = Kp*KLF*Kv*Wp1*Wp2/(Wz*N);
p = [0;-Wp1;-Wp2];
z = [-Wz];
[num,den] = zp2tf(z,p,K);
PhiS = -180/(twopi*Fref)*ZH; % multiplier to get phase for hold (0 if ZH = 0)

clf;
fratio = (fstop/fstart)^(1/50);
for i = 1:51
	f(i) = fstart*fratio^(i-1);
end % creation of geometric (log) frequency vector

% Compute basic Bode response
w=twopi*f; 
[mag,phase,w] = bode(num,den,w);
	%if MATLAB 4, invert w to make compatible with MATLAB 5
	pdim = size(w); %dimensions of w matrix
	if pdim(1) == 1, % then w is row vector produced by MATLAB 4
		w = w'; % invert row vetor to column vector
	end;
phase = deg2rad*(phase + w*PhiS); % add effect of hold (if ZH is set)
if ZH % include effect of hold
	mag = sinc(w/ws).*mag; 
end
cmplx1 = mag.*(cos(phase) + 1i*sin(phase)); % convert mag and phase to complex value

% Compute for frequencies higher by Fref
w2 = w + ws; % add sample frequency to every element in array (vector)
[mag,phase,w2] = bode(num,den,w2); % at shifted frequency in w2
	%if MATLAB 4, invert w to make compatible with MATLAB 5
	pdim = size(w); %dimensions of w matrix
	if pdim(1) == 1, % then w is row vector produced by MATLAB 4
		w = w'; % invert row vetor to column vector
	end;
phase = deg2rad*(phase + w2*PhiS);
if ZH
	mag = sinc(w2/ws).*mag;
end
cmplx2 = mag.*(cos(phase) + 1i*sin(phase));

% Compute for frequencies lower by Fref
%  Compute at negative of desired frequencies to avoid problems with MATLAB
%  Use fact that real part of G is even function of frequency and imaginary
%   part is odd function of frequency to obtain at desired negative frequencies
w3 = ws - w; % negative of desired frequency
[mag,phase,w3] = bode(num,den,w3); % compute at positive frequencies, avoid error messages
	%if MATLAB 4, invert w to make compatible with MATLAB 5
	pdim = size(w); %dimensions of w matrix
	if pdim(1) == 1, % then w is row vector produced by MATLAB 4
		w = w'; % invert row vetor to column vector
	end;
phase = -deg2rad*(phase + w3*PhiS); % odd function of frequency
if ZH
	mag = sinc(w3/ws).*mag;
end
cmplx3 = mag.*(cos(phase) + 1i*sin(phase));

% add the three parts
cmplxTt = cmplx1 + cmplx2 + cmplx3;

clf
while op
	op = input('Enter 0 = quit; 1 = open-loop; 2 = error; 3 = output responses: ');
	if op~= 0,
	
		if (op == 3)|(op == 2),
			cmplxT =  cmplx1./(1+cmplxTt); % element by element division of left array by right array
		end
		
		if op == 3,
			cmplxT = N * cmplxT; % Eq. (7.28)
		elseif op==2
			cmplxT = 1 - cmplxT; % Eq. (7.29)
		else
			cmplxT = cmplxTt;
		end
		
		mag = abs(cmplxT);
		phase = angle(cmplxT);
		phase = unwrap(phase)/deg2rad; % prevent 360¡ discontinuities

		subplot(211), semilogx(w/2/pi,20*log10(mag));
		xlabel('Frequency (Hz)'); ylabel('Gain (dB)');
		grid;
	
		if op == 3,
			title('Closed-Loop Output Response at Input Frequency')
		elseif op==2
			title('Closed-Loop Error Response at Input Frequency')
		else
			title('Open-Loop Response')
		end % if

		subplot(212), semilogx(w/2/pi,phase);
		xlabel('Frequency (Hz)'), ylabel('Phase (degrees)');
		grid;
		
	end % if op~=0
end % while

% Script can be shortened if customized for MATLAB version 4 or 5.
% 	For 5, remove 5 lines 3 places (58-62; 72-76; 89-93) following bode() functions;
%		they have no effect in MATLAB 5, since IF -> false..
%	For 4, retain only the inversion, w = w' in each group of 5 lines, since IF -> true.

