%Bode Plot from open-loop equation
% 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.23) in FS by PL
% Choose presence of zero-order hold
% Additional comments are in BdT1o3.m

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

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;',Kp,KLF,Kv);
fprintf('\nFp1 = %g Hz; Fz = %g Hz; Fp2 = %g Hz;',Fp1,Fz,Fp2);
fprintf('\nFref = %g Hz; N goes from %g to %g;',Fref, N0, N1);
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);
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; % clear previous graphs; for v.3.5, clg ######### 

% change next 2 lines to change size of frequency step
fratio = (fstop/fstart)^(1/150);
for i = 1:151
	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
cmplxT = cmplx1 + cmplx2 + cmplx3;
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 for N=1 (dB)');
grid;
v = axis; % vector of automatic axis ranges 
x = [v(1); v(2)]; % 2 element x vector to put N level across graph
y0 = 20*log10(N0);
y = y0*[1;1];
hold on; plot(x,y,'r-'); hold off;
a = '0 dB at N = ';
b = num2str(N0);
a(13:12+size(b,2)) = b;
text(fstart,y0,a);
y1 = 20*log10(N1);
y = y1*[1;1];
hold on; plot(x,y,'r-'); hold off;
a = '0 dB at N = ';
b = num2str(N1);
a(13:12+size(b,2)) = b;
text(v(2),y1,a,'HorizontalAlignment','right');
subplot(212), semilogx(w/2/pi,phase);
xlabel('Frequency (Hz)'), ylabel('Phase (degrees)');
grid;

% 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() function; they have no effect in MATLAB 5.
%	For 4, retain only the inversion, w = w' in each group of 5 lines, since IF test -> true.

