function [numopen, denopen, denclsd] = frqpid(num, den)
% Hadi Saadat,  1998

%discr=[
%'                                                                           '
%'  The function [numopen,denopen,denclsd]=frqpid(num, den) is used for the  '
%'  frequency response design of a PID controller.   num & den are           '
%'  row vectors of polynomial coefficients of the uncompensated open-loop    '
%'  plant transfer function.  Design is based on the phase margin criterion. '
%'  The user is prompted to enter the gain KI to achieve a desired steady-   '
%'  state error and the desired Phase Margin. The program finds and displays '
%'  a compensated gain crossover frequency range for an stable controller.   '
%'  The user is then prompted to enter the gain crossover frequency in this  '
%'  range.  The controller transfer function and the frequency-domain        '
%'  specifications before and after compensation are found.  The function    '
%'  returns the open-loop and the closed-loop numerator and denominator of   '
%'  the compensated system transfer function.                                '];

%disp(discr);

r=abs(roots(den));
i=find(r>0); rp=r(i);
rmx=max(rp); rmn=min(rp); wst=0.1*round(rmn); wf=20*round(rmx);;dw=wf/800;
w=wst:dw:wf;

clc
KI=input('Enter the integrator gain KI -> ');
pm=input('Enter the desired phase margin -> ');

[mag, phase] = bode(num, den, w);
phase=180/pi*unwrap(phase*pi/180);

if phase(1) > (-180+pm)
     i = find(phase < (-180 + pm));
else
     i = find(phase > (-180 + pm));
end
if length(i)==0
     disp('Phase does not cross (-180 + P.M.).')
return, else
     i2=i(1); i1 = i2 -1;
     if i1 ==0 wpm=w(i2); else
     wa = w(i1); wb = w(i2); p1 = phase(i1); p2 = phase(i2);
     wpm = wa + (-180+pm - p1)/(p2-p1)*(wb-wa);
     end
end
w1= wpm;

[M,ph]=bode(num, den, w1);  % Returns the mag. and phase of G(w)H(w1)
thta=-180 + pm - ph;
thtar=thta*pi/180;
wmx=round(10*w1); dw1=wmx/100;
wmn=w1/10; dw2=w1/10;
stab=0;
while sin(thtar)/(M*w1) +KI/(w1^2) < 0 | cos(thtar) < 0  & w1 < wmx
  w1=w1+dw1;
  [M,ph]=bode(num, den, w1);  % Returns the mag. and phase of G(w)H(w1)
  thta=-180 + pm - ph;
  thtar=thta*pi/180;
end
wmp=w1;
while sin(thtar)/(M*w1)+KI/(w1^2) > 0 & cos(thtar) > 0  & w1>wmn
  stab=stab+1;
  w1=w1-dw2;
  [M,ph]=bode(num, den, w1);  % Returns the mag. and phase of G(w)H(w1)
  thta=-180 + pm - ph;
  thtar=thta*pi/180;
end
w1mn=w1+dw2;
w1=wpm;
  [M,ph]=bode(num, den, w1);  % Returns the mag. and phase of G(w)H(w1)
  thta=-180 + pm - ph;
  thtar=thta*pi/180;
while sin(thtar)/(M*w1)+KI/(w1^2) >0 & cos(thtar) > 0 & w1 <wmx
  stab=stab+1;
  w1=w1+dw1;
  [M,ph]=bode(num, den, w1);  % Returns the mag. and phase of G(w)H(w1)
  thta=-180 + pm - ph;
  thtar=thta*pi/180;
end

w1mx=w1- dw1;
if stab==0
  fprint('Unstable controller change Phase KI or Margin and repeat')
  return
  else
  fprintf('For a stable controller select a compensated gain crossover\n')
  fprintf('frequency wgc between %7.3g',w1mn),fprintf(' and %7.3g\n',w1mx)
end

w1=input('Enter wgc -> ');
[M,ph]=bode(num, den, w1);  % Returns the mag. and phase of G(w)H(w1)
thta=-180 + pm - ph;
thtar=thta*pi/180;
Kp= cos(thtar)/M;
KD=KI/(w1^2) + sin(thtar)/(M*w1);

clc
fprintf('Uncompensated control system \n')
[Gm1, Pm1, wpc1, wgc1]=margin(mag, phase, w);
fprintf('Gain Margin  = %7.3g',Gm1),fprintf('    Gain crossover w = %7.3g\n',wgc1)
fprintf('Phase Margin = %7.3g',Pm1),fprintf('   Phase crossover w = %7.3g\n',wpc1)
fprintf('\n')
fprintf('Controller transfer function \n')
fprintf('     Gc = %g',Kp),fprintf(' + %g',KI),fprintf('/s + '),
fprintf('%g',KD),fprintf('s \n\n')

% the following statements will form the characteristic Equation
% of the compensated system.
m=length(num); n=length(den);
if n > m
o=zeros(1,n-m); mk=[o,1]; num1=conv(num,mk);
else, num1=num, end
numgc=[KD,Kp,KI]; numopen=conv(numgc,num1);
dengc=[0, 1, 0];  denopen=conv(dengc, den);
denclsd=denopen+numopen;
numopen=numopen(n-m+1:length(numopen));   % new 11/24/96
denopen =denopen(2:length(denopen));      % new
denclsd=denclsd(2:length(denclsd));       % new
%fprintf('Row vectors of polynomial coefficients of the compensated system:\n')
%fprintf('Open-loop num. '),disp(numopen)
%fprintf('Open-loop den. '),disp(denopen)
%fprintf('Closed-loop den'),disp(denclsd)
fprintf('Compensated open-loop ')
GH = tf(numopen, denopen)
fprintf('Compensated closed-loop ')
T = tf(numopen, denclsd)

[magp,phasep]=bode(numopen,denopen,w);
[Gm, Pm, wpc,wgc]=margin(magp,phasep,w);
if Pm>360 Pm=Pm-360; else, end
fprintf('Gain Margin  = %7.3g',Gm),fprintf('    Gain crossover w = %7.3g\n',wgc)
fprintf('Phase Margin = %7.3g',Pm),fprintf('   Phase crossover w = %7.3g\n',wpc)
fprintf('\n')
[M,ph]=bode(numopen,denclsd, w);
frqspec(w,M)

discr2=[
'Roots of the compensated characteristic equation:       '];
disp(discr2)
r=roots(denclsd);
disp(r)
rreal=real(r);
  for l=1:n-1
  if rreal(l) >=0
  fprintf('   Root on the RHP, system is unstable. Change KI or phase margin & repeat.\n\n')
  else,end
  end