% This script file computes the phase lead compensator
% for a given desired value of the phase margin

% First enter the desired steady-state error 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('Desired static gain Kb = %f\n',K)

% STEP 2.
% -------
omega=logspace(-1,2,800); % -1 and 2 can be different, depen on the plant
[Mag,Phase,w]=bode(pNum,pDen,omega);
[Gm,Pm,Wcp,Wcg]=margin(pNum,pDen);

% Print the results
fprintf('\n\nExisting system parameters with e_ss=%f\n\n',ess)
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',Wcp)
pause

% STEP 3.
% -------
if (Pm >= PhiDes)
     fprintf('\n\nTHE PHASE-LEAD 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 + 15;
     fprintf('Required phase margin reserve increase Pmax = %f degrees\n',PhiMax)

% STEP 4.   
% -------
     a=(1+sin(0.0174533*PhiMax))/(1-sin(0.0174533*PhiMax));
     fprintf('Parameter a = %f\n',a)

% STEP 5.
% --------
% Find wMax from the gain plot
     y=-10*log10(a);
 %    y=-15*log10(a);
     i=1;
     while 20*log10(Mag(i)) >= y
        i=i+1;
     end
     newWcg=w(i);
     fprintf('New value for Wcg = %f rad/s\n',newWcg)
% Solve for p2
     p2=newWcg*sqrt(a);
     cNum = [a p2];       % compensator numerator
     cDen = [1 p2];       % compensator denominator
     fprintf('The compensator pole p2 = %f\n',p2)
                       
% STEP 6.
% -------
% Cascade connection of the plant and lead compensator
     [Num,Den] = series(pNum,pDen,cNum,cDen);
     [nMag,nPhase,nW]=bode(Num,Den,omega);
     [nGm,nPm,nWcp,nWcg]=margin(Num,Den);
     fprintf('\n\nCompensated system parameters are as follows:\n\n')
     fprintf('Gain Margin              = %f dB\n',20*log10(nGm))
     fprintf('Phase Margin             = %f degrees\n',nPm)
     fprintf('Gain Crossover Frequency = %f rad/s\n',nWcg)
     fprintf('Phase Crossover Frequency= %f rad/s\n',nWcp)

% 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

% Now show the system transfer functions
     sprintf('The controller transfer function is:')
     printsys(cNum,cDen)
     sprintf('The new system transfer function:')
     printsys(Num,Den)
     pause

% Plot the step responses of uncompensated and compensated systems
     [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)

     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)
        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