% 
% 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: Lead and PID Controllers for Digital Control Systems')
disp('Names: Nolan Bushnell, Federico Faggin, Gordon E. Moore')
disp(' ')
disp(' ')

disp('PART 1: Lead 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 leadDz
syms  Golz leadDz

disp(' ')
disp('==== digital lead controller/compensator D(z) = 1 initially:  ')
Dz(z)=sym(1)
disp(' ')
disp('==== plant function Gp(s):  ')
Gp(s)=3600/((s+6/10)*(s+6)) 
disp(' ')
disp('==== feedback function H(s):  ')
H(s)=sym(1/10)
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('====  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/100         %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'))  ])
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('         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(' ')


 
disp('QQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQ ')
disp('Q1. FIRST,  ')
disp('    change the plant function Gp(s) above to Gp(s)=4000/((s+1/4)*(s+3)) , ')
disp('    and H(s)=sym(1/10), Dz(z)=sym(1), and T0=1/100.')
disp('    NOTE: digital filter Dz(z)=sym(1) above for an UNCOMPENSATED system.')
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(' ')
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('Q2. 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('    use the cursor tool in Fig. 2 to measure the following: ')
disp('         gain margin in dB   ')
disp('         phase margin in degrees  ')
disp(' ')
disp('Answer:  gain margin = ??? dB')
disp('Answer:  phase margin = ??? degrees')
disp(' ')
disp(' ')


disp('Q3. After making the changes in Q1,')
disp('    using Fig. 2 above,  ')
disp('    we will next design the phase-lead 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 degrees, and we will denote this frequency as "omega1lead" rad/s')
disp('    Use the cursor tool to measure the following: ')
disp('         the frequency where Gol(w) phase is -180 degrees')
disp(' ')
disp('Answer:  Set maatlab variable "omega1lead" here to the correct value: ')

omega1lead=1  % change this to the proper w-transform frequency 

a0=1/4  % dc gain of compensator
 
disp('Q4. After making the changes in Q1,')
disp('    in designing the phase-lead 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('Q5. After making the changes in Q1,')
disp('    using Fig. 2 above,  ')
disp('    in the design the phase-lead compensator (controller), ')
disp('    it is necessary to determine the desired phase of D(z) at  ')
disp('    frequency "omega1lead," equal to -180+phasemargin - openLooopPhase,  ')
disp('    where openLoopPhase equals the phase of Gol(w) at omega1lead rad/s,')
disp('    and the desired compensator phase is maatlab variable "leadThetaD".')
disp('    Use the cursor tool to measure the following: ')
disp('         what is the phase of Gol(w) at frequency omega1lead rad/s?')
disp('         what is the desired compensator phase "leadThetaD"?')
disp(' ')
disp('Answer:  phase of Gol(w) at frequency omega1lead rad/s = ??? degrees ')
disp('Answer:  Set maatlab variable "leadThetaD" here to the correct value: ')

leadThetaD=31  % change this to the proper desired compensator phase

 

disp('Q6. After making the changes in Q1,')
disp('    in designing the phase-lead compensator (controller),  ')
disp('    coefficients a1 and b1 can now be calculated using the formulas ')
disp('    in the lecture notes.   ')
disp('       What a1?')
disp('       What is b1?')
disp(' ') 
disp('Answer:  Set maatlab variable "a1" and "b1" here to the correct values: ')

 a1=1
 b1=0.1

 

disp('Lead-controller pole "leadWpole" and zero "leadWzero" are  ')
disp('   automatically calculated from a0, a1, and b1 as follows: ')
leadWpole=1/b1
leadWzero=a0/a1


disp('Calculate D(z)==============================')

disp('Q7. After making the changes in Q1,')
disp('    in designing the phase-lead compensator (controller),  ')
disp('    coefficient a0, and "leadWpole," and "leadWzero" ')
disp('    can be used to calculate D(z), as in the lecture notes. ')
disp('    Here, D(z)=leadKd*(z-leadZzero)/(z-leadZpole)')
disp('    where maatlab variables "leadKd," "leadZzero," and "leadZpole"')
disp('    set the dc gain, zero, and pole of D(z) respectively.')
disp('       What is leadKd?')
disp('       What is leadZzero?')
disp('       What is leadZpole?')
disp(' ') 
disp('Answer:  Set maatlab variables "leadKd," "leadZzero," and "leadZpole"  ')
disp('         here to the correct values: ')

 leadZzero=0.5
 leadZpole=0.2   
 leadKd=1




disp('========================================================')
disp('Automatically calculate D(z) from ki, kp, kd')
disp(' calculating phase-lead compensator z-domain pole and zero and constant Kd:') 
disp('lead compensator z-domain zero: ')    
leadZzero=vpa((1-leadWzero*T0/2)/(1+leadWzero*T0/2))
disp('lead compensator z-domain pole: ')    
leadZpole=vpa((1-leadWpole*T0/2)/(1+leadWpole*T0/2))
disp('lead compensator constant Kd: ')    
leadKd=vpa(a0*(leadWpole/leadWzero)*(1+leadWzero*T0/2)/(1+leadWpole*T0/2))

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

syms s z
disp('This is your phase-lead compensator design, leadDz(z): ')    
leadDz(z)=leadKd*(z-leadZzero)/(z-leadZpole);
olddigits=digits(); digits(5);
vpa(leadDz(z))
digits(olddigits);

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

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

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

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


disp(' ')
disp('==== Composite open loop gain leadDGolz(z) WITH COMPENSATOR D(z) ')
disp('     Gol(z)=D(z)+plant+feedback+zoh leadDGolz(z)=D(z)GHprimez(z)(z-1)/z:  ')
leadDGolz(z)=collect((subs(simplify(leadDz(z)*GHprimez(z)*(z-1)/z,'Steps',5))),z);
[num den]=numden(leadDGolz);coef=sym2poly(den);leadDGolznum =num/sym(coef(1));leadDGolzden=den/sym(coef(1));
leadDGolz(z)=leadDGolznum/leadDGolzden;
disp('Open loop gain WITH COMPENSATOR leadDGolz(z)=')
olddigits=digits(); digits(5);
vpa(leadDGolz(z))
digits(olddigits);
olddigits=digits(); digits(5);
leadDGolzstr = char(vpa(subs(leadDGolz))); %save for root locus printout
digits(olddigits);
disp('==== W-transform of composite open loop gain leadDGolw(w) WITH COMPENSATOR  ')
leadDGolw(w)=collect(simplify(subs(leadDGolz(z),z,(2+w*T0)/(2-w*T0)),'Steps',5),w);
[num den]=numden(leadDGolw);coef=sym2poly(den);leadDGolwnum =num/sym(coef(1));leadDGolwden=den/sym(coef(1));
leadDGolw(w)=leadDGolwnum/leadDGolwden;
disp('W-transform of open loop gain WITH COMPENSATOR leadDGolw(w)=')
olddigits=digits(); digits(5);
vpa(leadDGolw(w))
digits(olddigits);




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

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

TFstr = char(vpa(leadGclz)); 
z = tf('z',T0);
disp('======= transfer function form: '); 
% olddigits=digits(); digits(6);
eval(['leadGclzsys = ',TFstr]); 
leadGclzsys=series(leadGclzsys,tf([1],[1])); % fix a bug for when transfer function equals a number
[poles,zeroes] = pzmap(leadGclzsys);
disp([' COMPENSATED closed-loop leadGclzsys poles=' num2str(poles') '  and zeroes=' num2str(zeroes') ])
disp(['  magnitude of poles=' num2str(abs(poles')) '  and zeroes='])
disp(['==== Closed-loop dc gain (z=1) from leadGclz(z)= ' num2str(eval(leadGclz(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(leadDz(z))); z=tf('z', T0);
eval(['tfleadDz = ',TFstr]); 
tfleadGolz = series(tfGolz,tfleadDz) %open-loop gain
tfleadGfwdz = series(tfGfwdz,tfleadDz); %forward loop gain

disp('*********************   WARNING   **********************')
disp(' MAKE SURE that the symbolic closed loop gain "leadGclz" WITH COMPENSATOR and the transfer- ')
disp('     function form of closed loop gain "tfleadGclz" 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:')
tfleadGclz = feedback(tfleadGfwdz, tfHz)
disp('The tfleadGclz above should equal the symbolic leadGclz(z) here:')
syms z s
olddigits=digits(); digits(5); 
vpa(leadGclz(z))    %closed loop pulse trans func
digits(olddigits);
disp('****************  END of WARNING   *******************')

 

disp('==== W-transform of COMPENSATED composite open loop gain leadGolw(w) with D(z) ')
leadGolw(w)=collect(simplify(subs(leadDGolz(z),z,(2+w*T0)/(2-w*T0)),'Steps',5),w);
[num den]=numden(leadGolw);coef=sym2poly(den);leadGolwnum =num/sym(coef(1));leadGolwden=den/sym(coef(1));
leadGolw(w)=leadGolwnum/leadGolwden;
disp('W-transform of open loop gain WITH COMPENSATOR leadGolw(w)=')
olddigits=digits(); digits(5);
vpa(leadGolw(w))
digits(olddigits);
disp('==== W-transform of phase-lead compensator D(z) ')
leadDw(w)=collect(simplify(subs(leadDz(z),z,(2+w*T0)/(2-w*T0)),'Steps',5),w);
[num den]=numden(leadDw);coef=sym2poly(den);leadDwnum =num/sym(coef(1));leadDwden=den/sym(coef(1));
leadDw(w)=leadDwnum/leadDwden;
disp('W-transform of phase-lead compensator leadDw(w)=')
olddigits=digits(); digits(5);
vpa(leadDw(w))
digits(olddigits);

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

hh=figure(3);
bplt=bodeplot(tfleadGclz,'r',tfleadGolz,'b',tfGolz,'c',tfleadDz,'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-lead 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-lead 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), lead-compensated open-loop gain Gol(z) = D(z)Gcl(z),  ')
disp('         uncompensated open-loop gain Gol(z)=GcH(z), and the phase-lead')
disp('        compensator D(z) (maatlab transfer function tfleadDz).')
disp(' ')



disp('Q8. After making the changes in Q1 and entering the required design parameters,')
disp('    the bode plot in Fig. 3 above shows the compensated closed-loop ')
disp('    gain Gcl(z), compensated open-loop gain Gol(z)=D(z)Gcl(z), uncompensated')
disp('    open-loop gain Gol(z)=GcH(z), and the phase-lead 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-lead compensator')
disp(' ')
disp('Answer:  gain margin = ??? dB')
disp('Answer:  phase margin = ??? degrees')
disp('Answer:  Fig. 3 D(z) has the form of a phase-lead compensator: yes/no')
disp(' ')
disp(' ')


%hh=figure(4)
controlSystemDesigner({'rlocus','bode'},tfGpz,tfleadDz,tfHz)
disp(' Fig. 4. Performance of phase-lead compensated system, using controlSystemDesigner')
disp(' ')
pause(2)

disp('Q9. After making the changes in Q1 and entering the required design parameters,')
disp('    in Fig. 4 above,  ')
disp('    the controlSystemDesigner tool shows the open-loop gain margin, ')
disp('    open-loop phase margin, step ressponse, and root locus of the ')
disp('    phase-lead compensated system.')
disp('  !!! BEWARE: !!! ')
disp('       two different controlSystemDesigner windows will open ')
disp('       make sure that you are looking at the right window! ')
disp('    Use cursor cursor in control system designer 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('         step-response percent overshoot ')
disp('     Hint: you must use the open-loop gain curves')
disp('     Hint: your phase margin should within 5 degrees of your design goal')
disp(' ')
disp('Answer:  gain margin = ??? dB')
disp('Answer:  phase margin = ??? degrees')
disp('Answer:  step response percent overshoot: ??? % ')
disp(' ')
disp(' ')


disp('PART 2: PID Controllers for  Digital Control Systems ======')
disp(' ')
disp('  ')

%controlSystemDesigner(tfGpz,tfleadDz,tfHz)
disp('===========  PID design 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 leadDz
syms  Golz leadDz
disp(' ')
disp('==== Sample period and rate:  ')
T0=1/100         %sample period
fs=1/T0        %sample rate

disp(' ')
disp('==== digital lead controller/compensator D(z) = 1 initially:  ')
Dz(z)=2*(z+1)/z 
Dz(z)=sym(1)
disp(' ')
disp('==== plant function Gp(s):  ')
%Gp(s)=1/(s+2) 
% Gp(s)=sym(6)  %sol
% Gp(s)=20000/((s+1/2)*(s+5)) 
% Gp(s)=sym(6)  %sol
Gp(s)=3600/((s+6/10)*(s+6)) 
%Gp(s)=10000/(s*(s+20)*(s+50))
disp(' ')
disp('==== feedback function H(s):  ')
H(s)=sym(1/10)
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')
[Num,Den] = tfdata(tfGolz);syms z;      %convert to symbolic
Golz(z) = poly2sym(cell2mat(Num),z)/poly2sym(cell2mat(Den),z);
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(' ===============  PID controller design')
disp(' ')


disp('==== PID parameters')
kp=1
ki=2
kd=3

disp('PID controller:')
pidCtrl=pid(kp,ki,kd,0,T0,'IFormula','Trapezoidal','DFormula','BackwardEuler')
disp('PID controller transfer function form:')
tfPIDz=tf(pidCtrl)
tfPIDGolz = series(tfGolz,tfPIDz); %open-loop gain
tfPIDGfwdz = series(tfGpz,tfPIDz); %forward loop gain
disp('PID closed-loop gain in transfer function form:')
tfPIDGclz = feedback(tfPIDGfwdz, tfHz)
[poles,zeroes] = pzmap(tfPIDGclz);
disp([' tfPIDGclz poles=' num2str(poles') '  and zeroes=' num2str(zeroes') ])
disp(['  magnitude of poles=' num2str(abs(poles'))  ])




hh=figure(5);
bplt=bodeplot(tfPIDGclz,'r',tfPIDGolz,'b',tfGolz,'c',tfPIDz,'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 PID 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-lead 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. 5. 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 PID')
disp('        compensator D(z)')
disp(' ')


 
%hh=figure(6)
controlSystemDesigner({'rlocus','bode'},tfGpz,tfPIDz,tfHz)
disp(' Fig. 6. Performance of PID compensated system, using controlSystemDesigner.')
disp(' ')
disp(' ')
pause(2)


disp('Q10. In Fig. 5 and Fig. 6 above,  the plots of Fig. 5 and ')
disp('    the controlSystemDesigner tool show the open-loop gain margin, ')
disp('    open-loop phase margin, step response, and root locus of the ')
disp('    PID compensated system, with parameters "ki," "kp," and "kd" above.')
disp('    The system is the same as the PID example in lecture notes,')
disp('    so CHANGE THE VALUES of maatlab variables "ki," "kp," and "kd"  ')
disp('    above to the same as in the lecture notes example. ')
disp('     Hint: your plots should be the same as in lecture notes')
disp('     Hint: phase and gain margin should be the same as in lecture notes')
disp('  !!! BEWARE: !!! ')
disp('       two different controlSystemDesigner windows will open ')
disp('       make sure that you are looking at the right window! ')
disp('    Use cursor cursor in control system designer 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('         step-response percent overshoot ')
disp('     Hint: you must use the open-loop gain curves')
disp('     Hint: your phase margin should within 5 degrees of the design goal')
disp('     Hint: observe the  PID controller in Fig. 6')
disp(' ')
disp('Answer:  gain margin = approx 33.5 dB')
disp('Answer:  phase margin = approx 45 degrees')
disp('Answer:  step response percent overshoot: approx 24% ')
disp(' ')
disp(' ')