%Synthesizer Simulation
%Time Responses and Phase Plane
%Sample & Hold PD, 0-1 cycle range
%  Using matrix inverse for initial conditions

clear;

% MODIFY PARAMETERS BELOW & GO
%*****************************
Fref = 10000;  % Reference frequency in Hz
N1 = 1000;  % divide number during simulation
N0 = 750;  % prior divide number (at initial state)
PNL = .5;  % cycles final phase error, PHA
Kp = 3;  % PD gain, U/cycle
KLF = 60000;  % Filter DC gain
Fp1 = .01;  % Filter first pole in Hz
Fz = 300;  % Filter zero in Hz
Fp2 = 3000;  % Filter second pole in Hz
Kv = 1e6;  % VCO gain, Hz/U
Tend = 4.8e-3; % Student Matlab Fref*Tendt ² 1024(v3.5) or 8192(v4)
truncatePh = 1; % 1:truncate phase; 0:do not
Punits = 1;     % 0:radians  1:cycles  2:degrees
Funits = 1;     % 0:radians/sec   1:Hz
% Choose Output
%  op=1:  Phase Plane = Freq. vs. Phase
%  op=2:  Phase Plane + Phase Error vs. Time
%  op=3:  Frequency + PD Output vs. Time
%  op=4:  All of above = 4 plots
op = 4;
ACC = 1e-8;
%******************************
FNL = N1*Fref; F0 = N0*Fref; K = Kp*KLF*Kv;  
Pha0 = PNL+(F0 - FNL)/K; % Æ¿ * K = ÆF, Æ from initial to final
Poff = FNL/K - PNL;
Tref = 1/Fref;
fprintf('\nLoop with 0 - 1 cycle Sample-and-Hold Phase Detector');
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);
fprintf('\nFrequency from %g Hz to %g Hz;', F0,FNL);
fprintf('\nPhase Error from %g cycle to %g cycle;\n', Pha0,PNL);
twopi = 2*pi;
wp1 = twopi * Fp1;
wp2 = twopi * Fp2;
wz = twopi * Fz;
Kx = wp1*wp2*K/wz;
wps = wp1 + wp2;
wpm = wp1*wp2;

clf; % clear previous graphs; for v.3.5, clg #########

%Polynominals for open loop gain
% 3 outputs to equal the order of the equations so C will be square
% and can be inverted in obtaining initial conditions.
 N = [1    wz    0    0
      0    1    wz    0
      0    0     1    wz]*Kx; % numerator
 D = [1   wps   wpm   0 ]; % denominator

[a,b,c,d] = tf2ss(N,D); % elements of dynamic matrix equation
x0 = inv(c)*([0 F0 0]' - d*F0/K); % initial conditions
% produced by operating on {Y(0) - d*Pha0} with inverse of c.
% Y = [y1 y2 y3]'; y1 = dF/dt is zero in steady state.  y2 = F0 is 
% initial frequency.  y3 = K*Pha0 is initial output phase error; 
yywas = 0; % initial phase at VCO output
	
%    BEGIN SIMULATION

TT(1) = 0;
Pha(1) = Pha0; % phase
Phe(1) = Pha0; % PD output
FF(1) = F0; % frequency
i = 1;
T = Tref;
while TT(i)<Tend,
	u1(1) = Phe(i) + Poff; % input to sampler at start of period = held value
	u1(2) = u1(1);  % constant Hold output -> same at beginning and end of period
	y = 0; % y is phase change at divider output
	while abs(y-1)>ACC,
		[Phi, Gamma] = C2D(a,b,T); % values for use in solution eqution [see manual]
		x = ltitr(Phi,Gamma,u1',x0); % state variable solution for each value of t (only 2 in this case)
		yy = c*x'; % corresponding outputs  % (d is null for phase-transfer polynominal above)
		y = (yy(3,2) - yywas)/N1; % phase (row 3) at end of period (col. 2) is needed
		T = T/y; % iterate until 1 cycle phase change occurs in divider
	end; %while abs(y-1)
	yywas = yy(3,2);
	TT(i+1) = T + TT(i);
	Ph = Pha(i) + T*Fref - 1; % Phase at sample
	Pha(i+1) = Ph;
	Phe(i+1) = Ph - floor(Ph); % limit range of this variable to 0-1 cycle
	FF(i+1) = yy(2,2); % row 2 for frequency
	i = i+1;
	x0 = x(2,:)'; % final x' becomes next initial x
end; %while TT

% END SIMULATION.  The rest is plotting.

%  Set multipliers for chosen units
if Punits == 1,
	Pscale = 1;
elseif Punits == 2, 
	Pscale = 360;
else,
	Pscale = 2*pi;
end % if Punits

if Funits == 1,
	Fscale = 1;
else
	Fscale = 2*pi;
end % if Funits

%  Create each of the 4 plot types and locate them depending on what combination was chosen.

if op ~= 3, % selecting phase-plane plot
	if op==1,
		subplot(111),
	elseif op==2,
		subplot(211),
	elseif op==4,
		subplot(221),
	end % if op==1
	if truncatePh,
		plot(Pscale*Phe,Fscale*FF,'r')
		title('Phase Plane, Freq. vs. Phe')
	else
		plot(Pscale*Pha,Fscale*FF,'r')
		title('Phase Plane, Freq. vs. Pha')
	end; %if truncate
	grid
	if Punits == 1, % Set display of phase units
		xlabel('phase error (cycles)');
	elseif Punits == 2,
		xlabel('phase error (degrees)');
	else,
		xlabel('phase error (radians)')
	end % if Punits
	if Funits == 1, % Set display of frequency units
		ylabel('freq (Hz)');
	else,
	 ylabel('freq (rad/sec)');
	end % if Funits
end % if op~=3

if (op == 2)|(op == 4), % selecting phase error vs. time
	if op == 2,
		subplot (212),
	else,
		subplot (223),
	end % if op==2
	plot(TT,Pscale*Pha,'g')
	title('Pha, Phase Error')
	grid
	xlabel('seconds')
	if Punits == 1, % Set display of phase units
		ylabel('phase (cycles)');
	elseif Punits == 2,
		ylabel('degrees');
	else,
		ylabel('radians');
	end % if Punits
end % if ()|()

if (op == 3)|(op == 4), 
	if op == 3, % selecting frequency vs. time
		subplot(211);
	else,
		subplot(222),
	end % if op==3 
	plot(TT,Fscale*FF,'b')
	title('Frequency Out')
	grid
	xlabel('seconds');
	if Funits == 1, % Set display of frequency units
		ylabel('Hz');
	else,
	 ylabel('radians / second');
	end % if Funits

	if op == 3, % selecting PD output vs. time
		subplot(212);
	else,
		subplot(224),
	end % if op==3 
	plot(TT,Phe)
	title('Phe, PD Output/(Kp cycle)')
	grid
	xlabel('seconds')
end % if ()|()


