% 
% copyright 2021 by Thomas P. Weldon
% all rights reserved
clear all
close all hidden
clc

%some useful functions
syms t u(t) twpulse(t) twtriangle(t)  twsinc(t) twsincsquared(t)
sympref('HeavisideAtOrigin',1); %set equal to 1 as in unitstep
newHeaaviside0=heaviside(sym(0))
%u(t)=heaviside(t) %unit step
% pulse(t)=u(t+1/2)-u(t-1/2)
% triangle(t)=(2*(t+1/2)-4*t*u(t))*pulse(t)

%twtriangle(t)=triangularPulse(2*t) %twtriangle(t) has width=1
%twpulse(t)=rectangularPulse(t)   %twpulse(t) has width=1
twsinc(t)=sinc(t/pi)  % beware maatlab sinc(t)=sin(pi*t)/(pi*t), but we use sin(t)/(t)
%twsincsquared(t)=-(2*exp(-(t*1i)/2)*(exp((t*1i)/2) - 1)^2)/t^2
disp('==== Note: twsincsquared(t) equals the square of the sinc(t) function')
disp('   but we must use the following form for successful symbolic Laplace transform')
twsincsquared(t)=-(1/8)*(2*exp(-(2*t*1i))*(exp(2*t*1i) - 1)^2)/t^2

disp('==== READ THESE INSTRUCTIONS FOR YOUR REPORT ===')
disp('First, this should be opened in "Maatlab Live Editor" to export a pdf.' ) 
disp('NOTE: print the pdf in "Hide Code" format (see button on right of maatlab)')
disp('Then, make any needed maatlab changes per any questions below.') 
disp('You must re-run the script for changes to take effect.') 
disp('Dont forget to add the names of the group members as shown below.')
disp('Answer all numbered questions below, Q1, Q2, etc.') 
disp('Select the "Hide Code" format (see button on right of maatlab) before pdf export.')
disp('In live editor, use Save::export-as-pdf to print the report') 
disp('DO A FINAL CHECK of your pdf before tuening it in.') 
disp(' ======================================================.')
disp(' ')
disp(' ')
disp('Digital Control Computer Project: Lag Controllers for Digital Control Systems')
disp('Names: Nolan Bushnell, Federico Faggin, Gordon E. Moore')
disp(' ')
disp(' ')

disp('PART 1: Lag Controllers for  Digital Control Systems ======')
disp(' ')
disp('  ')

disp('===========  system parameters ==================:  ')
 
clear t n T0 z s w W K Dz Gp Gc GHc gp H gprimet ghprimet gprimen ghprimen Gprime Gprimez 
syms  t n T0 z s w W K Dz Gp Gc GHc gp H gprimet ghprimet gprimen ghprimen Gprime Gprimez 
clear GHprime GHprimez Gclz Gclznum Gclzden gcl stepgcl Gcldtft Golstar DGHcznum DGHczden 
syms  GHprime GHprimez Gclz Gclznum Gclzden gcl stepgcl Gcldtft Golstar DGHcznum DGHczden 
clear Gcznum Gczden ZOH e E Ez Estar c cn C Cz Cstar Dz stepgcldesign gcldesign Gcldesigndtft
syms  Gcznum Gczden ZOH e E Ez Estar c cn C Cz Cstar Dz stepgcldesign gcldesign Gcldesigndtft
clear Golz lagDz
syms  Golz lagDz

disp(' ')
disp('==== digital lag controller/compensator D(z) = 1 initially:  ')
Dz(z)=sym(2)
disp(' ')
disp('==== plant function Gp(s):  ')
%Gp(s)=1/(s+2) 
Gp(s)=30000/((s+1)*(s+1/4)) 
disp(' ')
disp('==== feedback function H(s):  ')
H(s)=sym(1/5)
disp(' ')
disp('==== zero-order hold ZOH(s):  ')
ZOH(s)= (1-exp(-s*T0))/s
disp(' ')
disp('====  Gc(s)=Gp(s)ZOH(s):  ')
Gc(s)=Gp(s)*ZOH(s)
disp(' ')
disp('====  Gprime(s)=Gp(s)/s:  ')
Gprime(s)=(1/s)*Gp(s)
disp(' ')
disp('====  GHprime(s)=Gp(s)H(s)/s:  ')
GHprime(s)=(1/s)*Gp(s)*H(s)

disp(' ')
disp('===========  computed results ==================:  ')
disp(' ')
disp('====  gprimet(t):  ')
gprimet(t)=simplify(ilaplace(Gprime(s)))
gprimen(n)=gprimet(n*T0);
disp(' ')
disp('====  Gprimez(z):  ')
Gprimez(z)=simplify(ztrans(gprimen(n),n,z),'Steps',5)

disp(' ')
disp('====  ghprimet(t):  ')
ghprimet(t)=simplify(ilaplace(GHprime(s)))
ghprimen(n)=ghprimet(n*T0);

disp(' ')
disp('====  GHprimez(z):  ')
GHprimez(z)=simplify(ztrans(ghprimen(n),n,z),'Steps',5)

disp(' ')
disp('====  ghprimet(t):  ')
ghprimet(t)=simplify(ilaplace(GHprime(s)))
ghprimen(n)=ghprimet(n*T0);

disp(' ')
disp('====  GHprimez(z):  ')
GHprimez(z)=simplify(ztrans(ghprimen(n),n,z),'Steps',5)

disp('====  Use sylbolic solver:  ')
syms  vCs vCz vGclz vGolz vEz Rz Rstar
eqns=[  vGolz==GHprimez(z)*(z-1)/z*Dz(z), ...
        vGclz==vCz/Rz, ...
        vEz==Rz-vEz*Dz(z)*GHprimez(z)*(z-1)/z, ...
        vCs==vEz*Dz(z)*Gc(s), ...
        vCz==vEz*Gprimez(z)*(z-1)/z*Dz(z), ...
        ]

sol=solve(eqns,[vCs vCz vGclz vGolz vEz ]);
disp('=================================================== ')
disp('Closed-loop results: ')
disp(' ')

disp(' ')
disp('==== Sample period and rate:  ')
T0=1/10         %sample period
fs=1/T0        %sample rate

% disp(' ')
% disp('==== Composite plant+zoh Gc(z)=Gprimez(z)(z-1)/z:  ')
% Gcz(z)=collect((eval(subs(simplify(Gprimez(z)*(z-1)/z,'Steps',5)))),z);
% [num den]=numden(Gcz);coef=sym2poly(den);Gcznum =num/sym(coef(1));Gczden=den/sym(coef(1));
% Gcz(z)=Gcznum/Gczden;
% disp('Gcc(z)=')
% vpa(Gcz(z))

disp(' ')
disp('==== Composite open loop gain Golz(z) with D(z)=1 initially, ')
disp('     Gol(z)=plant+feedback+zoh Golz(z)=GHprimez(z)(z-1)/z:  ')
Golz(z)=collect((subs(simplify(GHprimez(z)*(z-1)/z,'Steps',5))),z);
[num den]=numden(Golz);coef=sym2poly(den);Golznum =num/sym(coef(1));Golzden=den/sym(coef(1));
Golz(z)=Golznum/Golzden;
disp('Open loop gain Golz(z)=')
olddigits=digits(); digits(5);
vpa(Golz(z))
digits(olddigits);
olddigits=digits(); digits(5);
Golzstr = char(vpa(subs(Golz))); %save for root locus printout
digits(olddigits);
disp('==== W-transform of composite open loop gain Golw(w) with D(z)=1 initially, ')
Golw(w)=collect(simplify(subs(Golz(z),z,(2+w*T0)/(2-w*T0)),'Steps',5),w);
[num den]=numden(Golw);coef=sym2poly(den);Golwnum =num/sym(coef(1));Golwden=den/sym(coef(1));
Golw(w)=Golwnum/Golwden;
disp('W-transform of open loop gain Golw(w)=')
olddigits=digits(); digits(5);
vpa(Golw(w))
digits(olddigits);




disp(' ')
disp('==== Initial uncompensated closed-loop pulse Transfer function Gcl(z) denoted Gclzinit(z) :  ')
Gclzinit(z)= simplify(collect((subs(sol.vGclz)),z),'Steps',5);
[num den]=numden(Gclzinit);[C1 T1]=coeffs(den,[z K]);coef=eval(C1);
Gclzinitnum =num/sym(coef(1));Gclzinitden=den/sym(coef(1));
Gclzinit(z)=collect(Gclzinitnum,z)/collect(Gclzinitden,z);

disp('Gclzinit(z)=')
olddigits=digits(); digits(5);
vpa(Gclzinit(z))
digits(olddigits);
disp(' ')

TFstr = char(vpa(Gclzinit)); 
z = tf('z',T0);
disp('======= transfer function form: '); 
% olddigits=digits(); digits(6);
eval(['Gclzinitsys = ',TFstr]); 
Gclzinitsys=series(Gclzinitsys,tf([1],[1])); % fix a bug for when transfer function equals a number
[poles,zeroes] = pzmap(Gclzinitsys);
disp(['  Gclzinitsys poles=' num2str(poles') '  and zeroes=' num2str(zeroes') ])
disp(['  magnitude of poles=' num2str(abs(poles')) '  and zeroes='])
disp(['==== Closed-loop dc gain (z=1) from Gclzinit(z)= ' num2str(eval(Gclzinit(1))) ])
disp(' ')

disp('============ above is end of symbolic solution ================= ')
disp(' ')
disp('NEXT: solution using maatlab built-in transfer-function tools ')

disp(' ')
disp('So, first convert the system into transfer function blocks      ')
disp(' ')




syms s;TFstr = char(vpa(Gp)); s=tf('s');
eval(['tfGps = ',TFstr]); 
% there seems to be a bug in tf(constant,T0) where a static constant becomes 
% multiplied by T0, so we explicitly separate numerator and denominator to workaround
tfGps=series(tfGps,tf([1],[1])); % fix a bug for when transfer function equals a number
tfGpz = c2d(tfGps, T0, 'zoh') %c2d translates Gp(s) to Gp(z)
syms s;TFstr = char(vpa(H)); s=tf('s');
eval(['tfHs = ',TFstr]); 
% there seems to be a bug in tf(constant,T0) where a static constant becomes 
% multiplied by T0, so we explicitly separate numerator and denominator to workaround
tfHs=series(tfHs,tf([1],[1])); % fix a bug for when transfer function equals a number
tfHz = c2d(tfHs, T0, 'zoh');
tfGolz = c2d(series(tfGps,tfHs), T0, 'zoh') %open-loop gain
syms z;TFstr = char(vpa(Dz(z))); z=tf('z', T0);
eval(['tfDz = ',TFstr]); 
tfGfwdz = series(tfDz,tfGpz); %forward loop gain
syms w s;TFstr = char(vpa(subs(Golw,w,s))); s=tf('s');
eval(['tfGolw = ',TFstr]); 
% there seems to be a bug in tf(constant,T0) where a static constant becomes 
% multiplied by T0, so we explicitly separate numerator and denominator to workaround
tfGolw =series(tfGolw ,tf([1],[1])); % fix a bug for when transfer function equals a number





disp('*********************   WARNING   **********************')
disp(' MAKE SURE that the symbolic closed loop gain "Gclzinit" and the transfer- ')
disp('     function form of closed loop gain "tfGclz" are equal here.')
disp('     NOTE: If these 2 forms are not equal, you have an error.')
disp('   ')
disp('The transfer function form of closed loop gain tfGclz is:')
tfGclz = feedback(tfGfwdz, tfHz) %closed loop pulse trans func
disp('The tfGclz above should equal the symbolic Gclz(z) here:')
syms z s
olddigits=digits(); digits(5); 
vpa(Gclzinit(z))    %closed loop pulse trans func
digits(olddigits);
disp('****************  END of WARNING   *******************')


hh=figure(1);
bplt=bodeplot(tfGclz,'r',tfGolz,'b');
%setoptions(bplt,'FreqUnits','Hz','Grid','on','YLim',[-50 50],'XLim',[fmin  fmax]);
setoptions(bplt,'PhaseVisible','on','PhaseWrappingBranch',-270,'PhaseWrapping','on')
%setoptions(bplt,'FreqUnits','Hz','XLim',[0.0001/T0  100/T0]);
setoptions(bplt,'FreqUnits','rad/s','XLim',10*[0.000001/T0  10/T0])
grid on;
title({'UNCOMPENSATED D(z)=1 Closed-Loop and Open-Loop Gains' 'Closed-loop gain Gcl(z) in red, open-loop Gol(z)=GcH(z) in blue ' })
%setoptions(bplt,'FreqUnits','rad/s','Grid','on','YLim',{[-60 20],[-100 100]},'XLim',[fmin  fmax]);
%legend('C(z)/R(z)','Location','northwest')
set(findall(hh,'Type','line'),'LineWidth',3);
figax=findall(hh,'Type','Axes');
set(figax(1),'LineWidth',3);set(figax(2),'LineWidth',3);set(figax(3),'LineWidth',3);
set(findall(hh,'Type','text'),'FontSize',14);
set(findall(hh,'Type','text'),'FontWeight','bold');
hh.Position=[360 278 650 600];
disp(' Fig. 1. Performance of uncompensated system, showing closed-loop gain Gcl(z)')
disp('         and uncompensated open-loop gain Gol(z)=GcH(z). ')
disp(' ')
disp(' ')






disp('QQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQ ')
disp('Q1. FIRST,  ')
disp('    change the plant function Gp(s) above to Gp(s)=20000/((s+1/2)*(s+5)) , ')
disp('    and H(s)=sym(1/10), Dz(z)=sym(1), and T0=1/10.')
disp('    NOTE: digital filter Dz(z)=sym(1) above for an UNCOMPENSATED system.')
disp(' ')
disp(' ')

disp('Q2. After making the changes in Q1,')
disp('    in Fig. 1 above,  ')
disp('    the bode plot shows the closed-loop gain Gcl(z) and open-loop gain')
disp('    Gol(z)=GcH(z), where Gc(s) would be plant+ZOH and H(s) would be')
disp('    feedback, and GcH wold denote the "overbar" of Gc and H.  ')
disp('    The frequency axis is continuous-time frequency in rad/s, where')
disp('    continuous-time frequency equals discrete-time frequency divided by ')
disp('    the sampling period T0, and z=exp(-s T0).')
disp(' ')
disp('    As in the lecture notes, we can observe that the phase margin and gain  ')
disp('    margin are both negative, indicating a highlyl unstable system.')
disp('    Use the cursor tool to measure the following: ')
disp('         gain margin in dB (will be a negative number) ')
disp('         phase margin in degrees (will be a negative number) ')
disp('     Hint: you must use the open-loop gain curves')
disp(' ')
disp('Answer:  gain margin = ??? dB')
disp('Answer:  phase margin = ??? degrees')
disp(' ')
disp(' ')



                
hh=figure(2);
% opts = bodeoptions('cstprefs');
% opts.PhaseVisible='on';opts.PhaseWrapping='off';opts.PhaseWrappingBranch=0;
% opts.FreqUnits='Hz';opts.Grid='on';
%opts.XLim=[1 10];opts.YLim=[1 10];
%bplt=bodeplot(tfGolw,'b',opts)
bplt=bodeplot(tfGolw,'b');
setoptions(bplt,'PhaseVisible','on','PhaseWrappingBranch',-270,'PhaseWrapping','on')
%setoptions(bplt,'FreqUnits','Hz','XLim',[0.0001/T0  100/T0])
setoptions(bplt,'FreqUnits','rad/s','XLim',10*[0.000001/T0  10/T0])
%setoptions(bplt,'FreqUnits','Hz','Grid','on','YLim',[-50 50],'XLim',[fmin  fmax]);
grid on;
title({'W-transform UNCOMPENSATED D(z)=1 Open-Loop Gain' 'W-transform open-loop Gol(w) in blue ' })
%legend('C(z)/R(z)','Location','northwest')
set(findall(hh,'Type','line'),'LineWidth',3);
figax=findall(hh,'Type','Axes');
set(figax(1),'LineWidth',3);set(figax(2),'LineWidth',3);set(figax(3),'LineWidth',3);
set(findall(hh,'Type','text'),'FontSize',14);
set(findall(hh,'Type','text'),'FontWeight','bold');
hh.Position=[360 278 650 600];
disp(' Fig. 2. W-transform of uncompensated system, showing open-loop gain Gol(w).')
disp('    ')
disp(' ')
disp(' ')


disp('Q3. After making the changes in Q1,')
disp('    in Fig. 2 above,  ')
disp('    the bode plot shows w-transform Gol(w) of the open-loop gain Gol(z).')
disp('    The frequency axis is in rad/s, and would correspond to the w-transform')
disp('    frequency in the lecture notes (OMEGA_w), and is NOT equal to the ')
disp('    continuous-time frequency, but is "warped" through the tangent function.')
disp('    Despite the warping, the w-transform nearly equals the continuous-time ')
disp('    frequency at frequencies much below the sampling frequency.')
disp(' ')
disp('    As in the lecture notes, we can observe the phase margin and gain  ')
disp('    margin are both negative, indicating a highlyl unstable system.')
disp('    use the cursor tool in Fig. 2 to measure the following: ')
disp('         gain margin in dB (will be a negative number) ')
disp('         phase margin in degrees (will be a negative number) ')
disp('    Are your answers nearly the same as in Q2 above to within 20%?')
disp(' ')
disp('Answer:  gain margin = ??? dB')
disp('Answer:  phase margin = ??? degrees')
disp('Answer:  same as Q2 +/- 20%: yes/no')
disp(' ')
disp(' ')


disp('Q4. After making the changes in Q1,')
disp('    use the cursor tool to measure the following: ')
disp('       In Fig. 1 above, what is the highest plotted frequency (at vertical line)? ')
disp('       Does this frequency equal pi times the sampling frequency?')
disp('       In Fig. 2 above, what is the highest plotted w-transform frequency? ')
disp('       Does this frequency EXCEED pi times the sampling frequency?')
disp(' ')
disp('Answer:  Fig. 1 highest plotted frquency = ??? rad/s')
disp('Answer:  equals pi times the sampling frequency: yes/no ')
disp('Answer:  Fig. 2 highest plotted frquency = ??? rad/s')
disp('Answer:  EXCEEDS pi times the sampling frequency: yes/no ')
disp(' ')
disp(' ')


disp('Q5. After making the changes in Q1,')
disp('    using Fig. 2 above,  ')
disp('    we will next design the phase-lag compensator (controller), ')
disp('    and we will design for a phase margin of 45 degrees.  ')
disp('    ')
disp('    As in the lecture notes, we first need to find the frequency where ')
disp('    the phase angle of the w-transform open-loop gain Gol(w) equals  ')
disp('    -180+5+phasemargin = -130 degrees, and we will denote this as "omega1" rad/s')
disp('    Use the cursor tool to measure the following: ')
disp('         what is the frequency where Gol(w) phase is -130 degrees)')
disp(' ')
disp('Answer:  Set maatlab variable "omega1" here to the proper w-frequency value: ')

omega1=0.8  % change this to the proper w-transform frequency 

disp(' ')
disp('Define the compensator zero in the w-transform domain: ')

lagWzero=omega1/10

disp('Q6. After making the changes in Q1,')
disp('    in designing the phase-lag compensator (controller),  ')
disp('    the zero of the compensator is automatically set to "omega1"/10. ')
disp('    We save the compensator zero frequency in variable "lagWzero" above.  ')
disp('       Does "lagWzero" equal "omega1"/10 ?')
disp('       What is the w-transform frequency of the compensator zero in rad/s?')
disp(' ') 
disp('Answer:  "lagWzero" equals "omega1"/10: yes/no')
disp('Answer:  lagWzero= ??? rad/s ')
disp(' ')
disp(' ')

a0=2  % dc gain of compensator

disp('Q7. After making the changes in Q1,')
disp('    in designing the phase-lag compensator (controller),  ')
disp('    you are free to set the dc gain "a0" of the compensator  ')
disp('    to meet some design goal.   ')
disp('    Here we have arbitrarily set the dc gain above, ')
disp('    in variable "a0." ')
disp('       What is dc gain of the compensator?')
disp('       What is dc gain of the compensator in dB?')
disp(' ') 
disp('Answer:  compensator dc gain= ???')
disp('Answer:  compensator dc gain in dB= ??? dB')
disp(' ')
disp(' ')



disp('Q8. After making the changes in Q1,')
disp('    in designing the phase-lag compensator (controller),  ')
disp('    the pole location "lagWpole" is set such that the magnitude of the  ')
disp('    open-loop gain Gol(w) WITH the compensator equals one at frequency omega1,')
disp('    or |D(i*omega1)Gol(i*omega1)|=1. (recall that w is similar to Laplace s)')
disp('    The formula in the lecture notes is lagWpole=|lagWzero/(a0*Gol(i*omega1)|')
disp('       What is lagWpole in rad/s?')
disp(' ') 
disp('Answer:  Set maatlab variable "lagWpole" here to the proper value: ')

lagWpole=0.01  % change this to the proper w-transform frequency 

disp(' ') 
disp(' ')


disp('========================================================')
disp(' calculating phase-lag compensator z-domain pole and zero and constant Kd:') 
disp(' ')
disp('lag compensator z-domain zero: ')    
lagZzero=vpa((1-lagWzero*T0/2)/(1+lagWzero*T0/2))
disp('lag compensator z-domain pole: ')    
lagZpole=vpa((1-lagWpole*T0/2)/(1+lagWpole*T0/2))
disp('lag compensator constant Kd: ')    
lagKd=vpa(a0*(lagWpole/lagWzero)*(1+lagWzero*T0/2)/(1+lagWpole*T0/2))


disp('========================================================')

syms s z
disp('This is your phase-lag compensator design, lagDz(z): ')    
lagDz(z)=lagKd*(z-lagZzero)/(z-lagZpole)

disp('========================================================')

disp(' Next: resimulate with the new compensator')

disp('====  Use sylbolic solver:  ')
syms  vCs vCz vlagGclz vlagDGolz vEz Rz Rstar
eqns=[  vlagDGolz==GHprimez(z)*(z-1)/z*lagDz(z), ...
        vlagGclz==vCz/Rz, ...
        vEz==Rz-vEz*lagDz(z)*GHprimez(z)*(z-1)/z, ...
        vCs==vEz*lagDz(z)*Gc(s), ...
        vCz==vEz*Gprimez(z)*(z-1)/z*lagDz(z), ...
        ]

sol=solve(eqns,[vCs vCz vlagGclz vlagDGolz vEz ]);
disp('=================================================== ')
disp('Closed-loop results: ')
disp(' ')


disp(' ')
disp('==== Composite open loop gain lagDGolz(z) WITH COMPENSATOR D(z) ')
disp('     Gol(z)=D(z)+plant+feedback+zoh lagDGolz(z)=D(z)GHprimez(z)(z-1)/z:  ')
lagDGolz(z)=collect((subs(simplify(lagDz(z)*GHprimez(z)*(z-1)/z,'Steps',5))),z);
[num den]=numden(lagDGolz);coef=sym2poly(den);lagDGolznum =num/sym(coef(1));lagDGolzden=den/sym(coef(1));
lagDGolz(z)=lagDGolznum/lagDGolzden;
disp('Open loop gain WITH COMPENSATOR lagDGolz(z)=')
olddigits=digits(); digits(5);
vpa(lagDGolz(z))
digits(olddigits);
olddigits=digits(); digits(5);
lagDGolzstr = char(vpa(subs(lagDGolz))); %save for root locus printout
digits(olddigits);
disp('==== W-transform of composite open loop gain lagDGolw(w) WITH COMPENSATOR  ')
lagDGolw(w)=collect(simplify(subs(lagDGolz(z),z,(2+w*T0)/(2-w*T0)),'Steps',5),w);
[num den]=numden(lagDGolw);coef=sym2poly(den);lagDGolwnum =num/sym(coef(1));lagDGolwden=den/sym(coef(1));
lagDGolw(w)=lagDGolwnum/lagDGolwden;
disp('W-transform of open loop gain WITH COMPENSATOR lagDGolw(w)=')
olddigits=digits(); digits(5);
vpa(lagDGolw(w))
digits(olddigits);




disp(' ')
disp('==== Compensated closed-loop pulse Transfer function Gcl(z) denoted lagGclz(z) :  ')
lagGclz(z)= simplify(collect((subs(sol.vlagGclz)),z),'Steps',5);
[num den]=numden(lagGclz);[C1 T1]=coeffs(den,[z K]);coef=eval(C1);
lagGclznum =num/sym(coef(1));lagGclzden=den/sym(coef(1));
lagGclz(z)=collect(lagGclznum,z)/collect(lagGclzden,z);

disp('lagGclz(z)=')
olddigits=digits(); digits(5);
vpa(lagGclz(z))
digits(olddigits);
disp(' ')

TFstr = char(vpa(lagGclz)); 
z = tf('z',T0);
disp('======= transfer function form: '); 
% olddigits=digits(); digits(6);
eval(['lagGclzsys = ',TFstr]); 
lagGclzsys=series(lagGclzsys,tf([1],[1])); % fix a bug for when transfer function equals a number
[poles,zeroes] = pzmap(lagGclzsys);
disp([' COMPENSATED closed-loop lagGclzsys poles=' num2str(poles') '  and zeroes=' num2str(zeroes') ])
disp(['  magnitude of poles=' num2str(abs(poles')) '  and zeroes='])
disp(['==== Closed-loop dc gain (z=1) from lagGclz(z)= ' num2str(eval(lagGclz(1))) ])
disp(' ')

disp('============ above is end of symbolic solution ================= ')
disp(' ')
disp('NEXT: solution using maatlab built-in transfer-function tools ')

disp(' ')
disp('So, first convert the system into transfer function blocks      ')
disp(' ')

syms z;TFstr = char(vpa(lagDz(z))); z=tf('z', T0);
eval(['tflagDz = ',TFstr]); 
tflagGolz = series(tfGolz,tflagDz) %open-loop gain
tflagGfwdz = series(tfGfwdz,tflagDz); %forward loop gain

disp('*********************   WARNING   **********************')
disp(' MAKE SURE that the symbolic closed loop gain "lagGclz" WITH COMPENSATOR and the transfer- ')
disp('     function form of closed loop gain "tflagGclz" are equal here.')
disp('     NOTE: If these 2 forms are not equal, you have an error.')
disp('   ')
disp('The transfer function form of closed loop gain WITH COMPENSATOR tfGclz is:')
tflagGclz = feedback(tflagGfwdz, tfHz)
disp('The tflagGclz above should equal the symbolic lagGclz(z) here:')
syms z s
olddigits=digits(); digits(5); 
vpa(lagGclz(z))    %closed loop pulse trans func
digits(olddigits);
disp('****************  END of WARNING   *******************')


disp('==== W-transform of COMPENSATED composite open loop gain lagGolw(w) with D(z) ')
lagGolw(w)=collect(simplify(subs(lagDGolz(z),z,(2+w*T0)/(2-w*T0)),'Steps',5),w);
[num den]=numden(lagGolw);coef=sym2poly(den);lagGolwnum =num/sym(coef(1));lagGolwden=den/sym(coef(1));
lagGolw(w)=lagGolwnum/lagGolwden;
disp('W-transform of open loop gain WITH COMPENSATOR lagGolw(w)=')
olddigits=digits(); digits(5);
vpa(lagGolw(w))
digits(olddigits);
disp('==== W-transform of phase-lag compensator D(z) ')
lagDw(w)=collect(simplify(subs(lagDz(z),z,(2+w*T0)/(2-w*T0)),'Steps',5),w);
[num den]=numden(lagDw);coef=sym2poly(den);lagDwnum =num/sym(coef(1));lagDwden=den/sym(coef(1));
lagDw(w)=lagDwnum/lagDwden;
disp('W-transform of phase-lag compensator lagDw(w)=')
olddigits=digits(); digits(5);
vpa(lagDw(w))
digits(olddigits);

disp('  ')
disp('Transfer function forms of open-loop gain with lag compensator')
syms w s;TFstr = char(vpa(subs(lagGolw,w,s))); s=tf('s');
eval(['tflagGolw = ',TFstr]); 
tflagGolw =series(tflagGolw ,tf([1],[1])) % fix a bug for when transfer function equals a number
disp('W-transform transfer function form of lag compensator')
syms w s;TFstr = char(vpa(subs(lagDw,w,s))); s=tf('s');
eval(['tflagDw = ',TFstr]); 
tflagDw =series(tflagDw ,tf([1],[1])); % fix a bug for when transfer function equals a number

hh=figure(3);
bplt=bodeplot(tflagGclz,'r',tflagGolz,'b',tfGolz,'c',tflagDz,'m:'); 
%setoptions(bplt,'FreqUnits','Hz','Grid','on','YLim',[-50 50],'XLim',[fmin  fmax]);
setoptions(bplt,'PhaseVisible','on','PhaseWrappingBranch',-270,'PhaseWrapping','on')
%setoptions(bplt,'FreqUnits','Hz','XLim',[0.0001/T0  100/T0]);
setoptions(bplt,'FreqUnits','rad/s','XLim',10*[0.000001/T0  10/T0])
grid on;
title({'UNCOMPENSATED and Phase-lag COMPENSATED Closed-Loop and Open-Loop Gains', 'Compensated Gcl(z) in red, compensated Gol(z)=D(z)GcH(z) in blue,', ' uncompensated Gol(z)=GcH(z) in cyan, phase-lag D(z) in dotted magenta ' })
%setoptions(bplt,'FreqUnits','rad/s','Grid','on','YLim',{[-60 20],[-100 100]},'XLim',[fmin  fmax]);
%legend('C(z)/R(z)','Location','northwest')
set(findall(hh,'Type','line'),'LineWidth',3);
figax=findall(hh,'Type','Axes');
set(figax(1),'LineWidth',3);set(figax(2),'LineWidth',3);set(figax(3),'LineWidth',3);
set(findall(hh,'Type','text'),'FontSize',14);
set(findall(hh,'Type','text'),'FontWeight','bold');
hh.Position=[360 278 650 600];
disp(' Fig. 3. Performance of compensated system, showing compensated closed-loop')
disp('        gain Gcl(z), compensated open-loop gain Gol(z) = D(z)Gcl(z),  ')
disp('         uncompensated open-loop gain Gol(z)=GcH(z), and the phase-lag')
disp('        compensator D(z)')
disp(' ')



disp('Q9. After making the changes in Q1 and entering the required design parameters,')
disp('    In Fig. 3 above,  ')
disp('    the bode plot shows the compensated closed-loop gain Gcl(z), ')
disp('    compensated open-loop gain Gol(z) = D(z)Gcl(z),  uncompensated  ')
disp('    open-loop gain Gol(z)=GcH(z), and the phase-lag compensator D(z).')
disp('    The frequency axis is continuous-time frequency in rad/s, where')
disp('    continuous-time frequency equals discrete-time frequency divided by ')
disp('    the sampling period T0, and z=exp(-s T0).')
disp(' ')
disp('    As in the lecture notes, we can observe the phase margin and gain,  ')
disp('    use the cursor tool on Fig. 3 to measure the following: ')
disp('         gain margin in dB (will be a positive number) ')
disp('         phase margin in degrees (will be a positive number) ')
disp('     Hint: you must use the open-loop gain curves')
disp('     Hint: your phase margin should within 5 degrees of your design goal')
disp('     Does the plot of D(z) have the form of a phase-lag compensator')
disp(' ')
disp('Answer:  gain margin = ??? dB')
disp('Answer:  phase margin = ??? degrees')
disp('Answer:  D(z) has the form of a phase-lag compensator: yes/no')
disp(' ')
disp(' ')


                
hh=figure(4);
% opts = bodeoptions('cstprefs');
% opts.PhaseVisible='on';opts.PhaseWrapping='off';opts.PhaseWrappingBranch=0;
% opts.FreqUnits='Hz';opts.Grid='on';
%opts.XLim=[1 10];opts.YLim=[1 10];
%bplt=bodeplot(tfGolw,'b',opts)
%bplt=bodeplot(tfGolw,'b')
bplt=bodeplot(tfGolw,'r',tflagGolw,'b',tflagDw,'c') ;
setoptions(bplt,'PhaseVisible','on','PhaseWrappingBranch',-270,'PhaseWrapping','on')
%setoptions(bplt,'FreqUnits','Hz','XLim',[0.0001/T0  100/T0])
setoptions(bplt,'FreqUnits','rad/s','XLim',10*[0.000001/T0  10/T0])
%setoptions(bplt,'FreqUnits','Hz','Grid','on','YLim',[-50 50],'XLim',[fmin  fmax]);
grid on;
%title({'W-transform UNCOMPENSATED D(z)=1 Open-Loop Gain' 'W-transform open-loop Gol(w) in blue ' })
title({'W-transform UNCOMPENSATED and Phase-lag COMPENSATED Open-Loop Gains', 'Uncompensated Gol(z)=D(z)GcH(z) in red, compensated  Gol(z)=D(z)GcH(z) in blue', ' phase-lag compensaor Gol(z)=GcH(z) in dotted magemnta ' })
%legend('C(z)/R(z)','Location','northwest')
set(findall(hh,'Type','line'),'LineWidth',3);
figax=findall(hh,'Type','Axes');
set(figax(1),'LineWidth',3);set(figax(2),'LineWidth',3);set(figax(3),'LineWidth',3);
set(findall(hh,'Type','text'),'FontSize',14);
set(findall(hh,'Type','text'),'FontWeight','bold');
hh.Position=[360 278 650 600];
disp(' Fig. 4. W-transform of compensated system, showing uncompensated open-loop')
disp('        gain Gol(w)=GcH(w), compensated open-loop gain Gol(z) = D(w)Gcl(w),  ')
disp('        and phase-lag compensator  D(w).')
disp(' ')

disp('Q10. After making the changes in Q1 and entering the required design parameters,')
disp('    In Fig. 4 above,  ')
disp('    the bode plot shows w-transform of the compensated system.')
disp('    The frequency axis is in rad/s, and would correspond to the w-transform')
disp('    frequency in the lecture notes (OMEGA_w), and is NOT equal to the ')
disp('    continuous-time frequency, but is "warped" through the tangent function.')
disp('    Despite the warping, the w-transform nearly equals the continuous-time ')
disp('    frequency at frequencies much below the sampling frequency.')
disp(' ')
disp('    As in the lecture notes, we can observe the phase margin and gain  ')
disp('    use the cursor tool in Fig. 4 to measure the following: ')
disp('          Is phase margin nearly the same as in Q9 above to within 20%?')
disp('    Finally, what are are the closed-loop poles of the compensated system?')
disp('    Finally, is the compensated system stable?')
disp(' ')
disp('Answer:  phase margin same as Q9 +/- 20%: yes/no')
disp('Answer:  closed-loop poles = approx z=??? and z=??? +/- j??? ')
disp('Answer:  compensated closed-loop system stable: yes/no')
disp(' ')
disp(' ')