% This script file computes the phase lead-lag controller
% for a given desired value of the phase margin

% First enter the desired steady-state eror and the desired phase margin.
% Then enter the numerator and denominator polynomials of the plant
% transfer function

clear
ess = input('Enter the desired steady-state error: ');
PhiDes=input('Enter the desired Phase Margin: ');
pNum=input('Enter the numerator polynomial of the plant: ');
pDen=input('Enter the denominator polynomial of the plant: ');

% STEP 1.
% -------
lNum=length(pNum);
lDen=length(pDen);
while pNum(lNum) == 0
    lNum = lNum-1;
end
while pDen(lDen) == 0
    lDen = lDen-1;
end
r=length(pDen)-lDen;

% Gain of the open-loop system to satisfy Ess
if ess ~= 0
   Ks = pNum(lNum)/pDen(lDen);
   if r==0
      K=pDen(lDen)*(1-ess)/(pNum(lNum)*ess); % unit step
   elseif r==1  
      K = 1/(Ks*ess);                        % unit ramp
   else
      K = 2/(Ks*ess);                        % unit parabola 
   end
% Multiply the numerator polynomial by K
   pNum=pNum*K;
end
fprintf('System is of type %d\n',r)
fprintf('Steady state gain K = %f\n',K)

% STEP 2.
% -------
omega=logspace(-1,2,800); % -1 and 2 can be different, depends on the plant
[Mag,Phase,w]=bode(pNum,pDen,omega);
[Gm,Pm,Wcp,Wcg]=margin(pNum,pDen);
% Printing of the results
fprintf('\nExisting system parameters are as follows:\n')
fprintf('Gain Margin              = %f dB\n',20*log10(Gm))
fprintf('Phase Margin             = %f degrees\n',Pm)
fprintf('Gain Crossover Frequency = %f rad/sec\n',Wcg)
fprintf('Phase Crossover Frequency= %f rad/sec\n\n',Wcp)
pause

% STEP 3.
% -------
if (Pm >= PhiDes)
     fprintf('\n\nTHE PHASE LEAD-LAG COMPENSATOR IS NOT NECESSARY!\n\n')
     pause
     figure(1)
     clf
     bode(pNum,pDen,omega)
     pause
     figure(2)
     clf
     [clpNum,clpDen]=cloop(pNum,pDen,-1)
     step(clpNum,clpDen)
     grid
else
     PhiMax = PhiDes - Pm;
     fprintf('Desired phase change PhiMax = %f degrees\n',PhiMax)
     a2=(1+sin(0.0174533*PhiMax))/(1-sin(0.0174533*PhiMax));
     fprintf('Parameter a2 = %f\n',a2)

% STEP 4.
% --------
% Find newWcg from the gain plot
%     y=1/sqrt(a2);
     y=-10*log10(a2);
     i=1;
     while 20*log10(Mag(i)) >= y
        i=i+1;
     end
     newWcg=w(i);
     fprintf('New crossover frequency newWcg = %f degrees\n',newWcg)

% STEP 5.
% --------
% Find p2
     p2=newWcg*sqrt(a2);
     z2=p2/a2;
     cNum_lead = [a2 p2];       % lead compensator numerator
     cDen_lead = [1 p2];        % lead compensator denominator
     fprintf('Pole of the lead part p2 = %f\n',p2)
     fprintf('Zero of the lead part z2 = %f\n',z2)
                  
% STEP 6.
% --------
     z1=0.1*z2;
     p1=z1/a2;
     fprintf('Pole of the lag part p1 = %f\n',p1)
     fprintf('Zero of the lag part z1 = %f\n',z1)
     cNum_lag=[1/a2 p1];
     cDen_lag=[1 p1];

% STEP 7.
% -------
% Transfer function of the phase lead-lag compensator
     [llNum,llDen] = series(cNum_lead,cDen_lead,cNum_lag,cDen_lag);
% The system transfer function
     sprintf('The lead part of the compensator is:')
     printsys(cNum_lead,cDen_lead)
     sprintf('The lag part of the compensator is:')
     printsys(cNum_lag,cDen_lag)
     sprintf('The controller transfer function is:')
     printsys(llNum,llDen)

% STEP 8.
% -------
% Cascade connection of the plant and lead compensator
     [Num,Den] = series(pNum,pDen,llNum,llDen);
     sprintf('The new system transfer function:')
     printsys(Num,Den)
     [nMag,nPhase,nW]=bode(Num,Den,omega);
     [nGm,nPm,nWcp,nWcg]=margin(Num,Den);
     fprintf('\nCompensated system parameters are as follows:\n')
     fprintf('Gain Margin              = %f dB\n',20*log10(nGm))
     fprintf('Phase Margin             = %f degrees\n',nPm)
     fprintf('Gain Crossover Frequency = %f rad/sec\n',nWcg)
     fprintf('Phase Crossover Frequency= %f rad/sec\n',nWcp)
     pause

% Bode plots of uncompensated and compensated systems
     figure(1)
     clf
     bode(pNum,pDen,omega)  % uncompensated system
     gtext('(a)')
     gtext('(a)')
     hold on
     bode(Num,Den,omega)    % compensated system
     gtext('(b)')
     gtext('(b)')
     hold off
     pause

% Plot the step response of the uncompensated and compensated system
     [clpNum,clpDen] =cloop(pNum,pDen,-1);
     [clNum,clDen]   =cloop(Num,Den,-1);
     [Y,x,time1]     =step(clpNum,clpDen);
     [Yc,xc,time2]   =step(clNum,clDen);
     if length(time1) < length(time2)
        time = time2;
     else
        time = time1;
     end
     [maxY,indY]=max(Y);
     [maxYc,indYc]=max(Yc);
     oY = (maxY-Y(length(Y)))/Y(length(Y))*100;
     oYc= (maxYc-Yc(length(Yc)))/Yc(length(Yc))*100;
     fprintf('\n\nUncompensated system overshoot = %f percent\n',oY)
     fprintf('Compensated system overshoot   = %f percent\n',oYc)
     pause
     figure(2)
     clf
     p=roots(clpDen);
     unst=0;
     for i=1:length(p),
         if real(p(i)) >0       % unstable system
            unst=1;
         end
     end
     if unst==0
        step(clpNum,clpDen,time)
 %      axis([0 6 0 2])
        gtext('(a)')
        hold on
        step(clNum,clDen,time)
        grid
        gtext('(b)')
        hold off
     else
        fprintf('\nUNCOMPENSATED SYSTEM UNSTABLE !\n')
        step(clNum,clDen,time)
        grid
     end
end
