% This program computes the phase lag compensator parameters
% for a given desired phase margin

% First enter the desired steady-state eror and the desired phase margin
% Then enter the numerator and denominator polynomial 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.  - Computing of Bode Gain
% -------

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('\nSystem 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\nStability margins for system with desired ess:\n\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',Wcp)
pause

if (Pm >= PhiDes)
     fprintf('\n\nTHE PHASE-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)
else
   
% Find Wcgnew from the phase Bode plot
     y=-180 + PhiDes + 7.5;
     i=1;
     while Phase(i) >= y 
        i=i+1;                    %the gain is -y
     end
     wcgnew=w(i);
     fprintf('New value for Wcg = %f rad/s\n',wcgnew)

% STEP 3.
% ---------
     a =1/Mag(i);
     fprintf('Parameter a = %f\n',a)

% STEP 4.
% ---------
% Solve for z1 and p1
     z1=0.1*wcgnew;
     p1=z1*a;
     cNum = [a p1];      % compensator numerator
     cDen = [1 p1];      % compensator denominator
     fprintf('The compensator zero z1 = %f\n',z1)
     fprintf('The compensator pole p1 = %f\n',p1)
                  
% STEP 5.
% -------
% Cascade connection of the plant and phase-lag 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/sec\n',nWcg)
     fprintf('Phase Crossover Frequency= %f rad/sec\n',nWcp)

% Bode diagrams of the 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)')
     pause
     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

% Find the step respose of the compensated and uncompensated 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) %choose the longer time
        time = time2;
     else
        time = time1;
     end

     [maxY,indY]=max(Y);
     [maxYc,indYc]=max(Yc);
     oY = (maxY-1)*100;
     oYc= (maxYc-1)*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


