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

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

disp('===========  system parameters ==================:  ')
 
clear t n T0 z s w W K Dz Gp Gc GHc Gcz gp H gprimet ghprimet gprimen ghprimen Gprime Gprimez 
syms  t n T0 z s w W K Dz Gp Gc GHc Gcz 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('==== plant function Gp(s):  ')
Gp(s)=2000/((s^2+s/4+80)*(s+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('==== Composite gain Gcz(z)=plant+zoh=Gprimez(z)(z-1)/z:  ')
Gcz(z)=collect((subs(simplify(Gprimez(z)*(z-1)/z,'Steps',5))),z);
[num den]=numden(Gcz);[C1 T1]=coeffs(den,[z K]);coef=eval(C1);
Gczinitnum =num/sym(coef(1));Gczinitden=den/sym(coef(1));
Gcz(z)=collect(Gczinitnum,z)/collect(Gczinitden,z);


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

 

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('Plant transfer function Gp(s): ')
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
disp('Composite transfer function Gc(z): ')
tfGcz = c2d(tfGps, T0, 'zoh') %c2d translates Gc(s) to Gc(z) with zoh



disp('*********************   WARNING   **********************')
disp(' MAKE SURE that the symbolic closed loop gain "Gcz(z)" and the transfer- ')
disp('     function form of the plant composite "tfGcz" are equal here.')
disp('     NOTE: If these 2 forms are not equal, you have an error.')
disp('   ')
disp('The transfer function form of Composite gain Gcz(z)=plant+zoh=Gprimez(z)(z-1)/z is:')
tfGcz=tfGcz %closed loop pulse trans func
disp('The tfGcz above should equal the symbolic Gcz(z)=plant+zoh here:')
syms z s
olddigits=digits(); digits(5); 
vpa(eval(Gcz(z)))    %closed loop pulse trans func
digits(olddigits);
disp('****************  END of WARNING   *******************')
 

 

[tfpoles,tfzeroes] = pzmap(tfGcz);
disp(['tfGcz poles=' num2str(tfpoles') '  and zeroes=' num2str(tfzeroes') ])
disp(['     magnitude of poles=' num2str(abs(tfpoles'))  ])
disp(['==== plant+zoh dc gain (z=1) from Gcz(z)= ' num2str(eval(Gcz(1))) ])
disp(' ')



hh=figure(1);
bplt=bodeplot(tfGcz,'b');
%setoptions(bplt,'FreqUnits','Hz','Grid','on','YLim',[-50 50],'XLim',[fmin  fmax]);
setoptions(bplt,'PhaseVisible','on','PhaseWrappingBranch',-270,'PhaseWrapping','off')
%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 Plant Pulse Transfer Function Gc(z)' 'Gc(z)=plant+zoh=Gprimez(z)(z-1)/z ' })
%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. ')
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)=1000/((s^2+s/2+40)*(s+1)). ')
disp('    In Fig. 1 above,  ')
disp('    the bode plot shows the UNCOMPENSATED Plant Pulse Transfer Function Gc(z)')
disp('    where Gc(z)=plant+zoh=Gprimez(z)(z-1)/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(' ')
     

disp('QQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQ ')
disp('Q2. After making the changes in Q1,  ')
disp('    is the plant transfer function Gp(s) stable? ')
disp(' ')
disp('Answer: plant transfer function Gp(s) stable: yes/no')
disp(' ')
disp(' ')
       

disp('QQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQ ')
disp('Q3. After making the changes in Q1,  ')
disp('    is the composite pulse transfer function Gcz(z) of the plant ')
disp('    stable?  ')
disp('    Note: Gcz(z)=plant+zoh=Gprimez(z)(z-1)/z')
disp(' ')
disp('Answer: Gcz(z) is stable: yes/no')
disp(' ')
disp(' ')
       


disp('============create state-space (state-variable) model: ')
disp(' ')
[A,B,C,D] = tf2ss(tfGcz.Numerator{1},tfGcz.Denominator{1});
svGc=ss(A,B,C,D,T0) % create state-space model 
svpoles=pole(svGc);
%[svpoles,svzeroes] = pzmap(tfGcz);
disp(['svGc poles=' num2str(svpoles')   ])
disp(['     magnitude of poles=' num2str(abs(tfpoles'))  ])
disp(' ')


disp('QQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQ ')
disp('Q4. After making the changes in Q1,  ')
disp('    are the poles of transfer function form "tfGcz"   ')
disp('    the same as the poles of the state-variable form "svGc"?  ')
disp(' ')
disp('Answer: poles of "tfGcz" same as poles of "svGc": yes/no')
disp(' ')
disp(' ')
       


disp(' ')
disp('check controllability: ')
contr = ctrb(A,B)   %check controllability

disp('QQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQ ')
disp('Q5. After making the changes in Q1,  ')
disp('    is the state-variable system "svGc" controllable? ')
disp('          Hint: try matlab function rank()  ')
disp(' ')
disp('Answer: controllable: yes/no')
disp(' ')
disp(' ')

disp(' ')
disp('check observability: ')
obs=  obsv(A,C)

disp('QQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQ ')
disp('Q6. After making the changes in Q1,  ')
disp('    is the state-variable system "svGc" observable? ')
disp('          Hint: try matlab function rank()  ')
disp(' ')
disp('Answer: observable: yes/no')
disp(' ')
disp(' ')


 
disp(' ')
disp('========Pole placement: ')
disp('location in s-plane of desired poles: ')
pdes=2*pi*[ -0.01/T0 -0.3/T0 -0.4/T0] %location in s-plane of desired poles


 
disp('QQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQ ')
disp('Q7. After making the changes in Q1,  ')
disp('    variable "pdes" above has the desired s-plane poles of the system. ')
disp('       Which of the poles is the dominant s-plane pole?  ')   
disp('    Hint: the lowest frequency or slowest-response pole is considered')
disp('          the "dominat pole"  because it predominantly slows down ')
disp('          the system response.  For example, exp(-2t) has slower decay')
disp('          than exp(-5t).')
disp(' ')
disp('Answer: dominant pole at s= ???  ')
disp(' ')
disp(' ')



pdesz=exp(pdes*T0) % location in z-plane

 
disp('QQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQ ')
disp('Q8. After making the changes in Q1,  ')
disp('    variable "pdesz" above has the desired z-plane poles of the system. ')
disp('       Which of the poles is the dominant z-plane pole?  ')   
disp(' ')
disp('Answer: dominant pole at z= ???  ')
disp(' ')
disp(' ')

disp('design controller feedback matrix/vector K: ')
K = place(A,B,pdesz)
AK=A-B*K %new A matrix after feedback
disp('design observer matrix/vector L:  ')
disp('     (Note: I use L where text uses G for state-variable observer) ')
L = place(A',C',exp(2.321*pdes*T0)) %observer design 5x as fast as controller
regstatevarz = reg(svGc,K,L') %regulator
GCLstatevar1z=feedback(svGc,regstatevarz,1);%must use positive feedback



disp('finally add gain correction factor: ')
% II=eye(length(A));
% gcor=(C*(II-(A-B*K))^(-1)*B)^-1; %correction for a state var controller
% gcor=(K*(II-(A-B*K-L'*C))^(-1)*L')^-1; %correction for a state var observer+controller
%directly find state-variable closed-loop Gcl(z) to compute correction gcor
[num den]=ss2tf(GCLstatevar1z.A,GCLstatevar1z.B,GCLstatevar1z.C,GCLstatevar1z.D); 
svGclz=tf(num,den,T0);  
gcor=freqresp(svGclz,0) %directly find dc gain (this is more accurate way to set dc gain
%gainBlock=tf([1/gcor],[1],T0)
gainBlock=tf([eval(Gcz(1))/gcor],[1],T0) %set gain to same as dc gain of plant
GCLstatevarz=series(gainBlock,GCLstatevar1z);%must use positive feedback
svpolesNew=pole(GCLstatevarz); % compute poles
svzeroesNew=zero(GCLstatevarz); %compute zeroes
disp(['GCLstatevarz poles=' num2str(svpolesNew')   ])
disp(['     magnitude of poles=' num2str(abs(svpolesNew'))  ])
disp(['desired z-plane poles=' num2str(pdesz)   ])


disp('check dc gain: ')
svdcgain=evalfr(GCLstatevarz,1)
 

hh=figure(2);
bplt=bodeplot(GCLstatevarz,'b',tfGcz,'r:');
%setoptions(bplt,'FreqUnits','Hz','Grid','on','YLim',[-50 50],'XLim',[fmin  fmax]);
setoptions(bplt,'PhaseVisible','on','PhaseWrappingBranch',-270,'PhaseWrapping','off')
%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({'Closed-Loop Gain with State-Variable Observer and Controller' 'State-Variable Compensated in solid blue, uncompensated plant in dotted red'  })
%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. 2. Closed-loop gain with state-variable observer and ')
disp('         controller.  State-Variable Compensated in solid blue, ')
disp('         uncompensated plant in dotted red. ')
disp(' ')
disp(' ')

 
disp('QQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQ ')
disp('Q9. After making the changes in Q1,  ')
disp('    Fig. 2 shows the closed-loop gain with state-variable observer and ')
disp('    controller in solid blue, and the uncompensated plant in dotted red. ')
disp('       Is the 3 dB bandwidth of the state-variable design larger than the')
disp('            3 dB bandwidth of the uncompensated plant?   ')   
disp('       Does the state-variable design exhibit less "peaking"?')
disp(' ')
disp('Answer: state-variable bandwidth is larger: yes/no  ')
disp('Answer: state-variable exhibits less "peaking": yes/no  ')
disp(' ')
disp(' ')

 
% figure(8) 
% pzmap(tfGcz,GCLstatevarz)

figure(3);
set(gca,'linewidth',3.0);
step(tfGcz,'r:',tf(GCLstatevarz),'b', 10);
%step(Gcl.s, '-', Gcl.z, '-r', 10);
title({'Step Response', 'Uncompensated in dotted red, and with Observer+Controller in solid blue'});
%legend('Continous-Time', 'Discrete-Time')
%title('e4112 closed-loop step response ')
aa=gca;
aa.XGrid='on';aa.YGrid='on';aa.LineWidth=3;aa.FontWeight='bold';
aa.XLabel.FontSize=20;aa.FontSize=12;
set(findall(gcf,'Type','line'),'LineWidth',3);
set(findall(gcf,'Type','text'),'FontSize',14);
set(findall(gcf,'Type','text'),'FontWeight','bold');
hh.Position=[360 278 650 600];

disp(' Fig. 3. Comparison of plant step response without controller, ')
disp('         to step response with observer+controller. Uncompensated  ')
disp('         plant in dotted red, and with Observer+Controller in solid blue.')
disp(' ')
disp(' ')


disp('QQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQQ ')
disp('Q10. After making the changes in Q1,  ')
disp('    Fig. 3 shows the step response with state-variable observer and ')
disp('    controller in solid blue, and the uncompensated plant in dotted red. ')
disp('       Is the rise time of the state-variable design faster than the')
disp('            rise time of the uncompensated plant?   ')   
disp('       Does the state-variable design exhibit less "overshoot"?')
disp(' ')
disp('Answer: state-variable rise time is faster: yes/no  ')
disp('Answer: state-variable exhibits less overshoot: yes/no  ')
disp(' ')
disp(' ')