%
%  Controller Tuning Utility
%
%  Uses IMC tuning rules in 
%    Chien and Fruehauf CEP Oct 1990 p33
%
%  Bob Newell, Oct 1993
%
%-------------------------------------------------------------------
%
%  read peak data
%
  peak = zeros(1,3) ; time = zeros(1,3) ;
  disp( 'Enter the time and value for 1st 3 extrema:' );
  for i = 1:3
    time(i) = input( [ '  Time  ' num2str(i) ' ? ' ] ) ;
    peak(i) = input( [ '  Value ' num2str(i) ' ? ' ] ) ;
  end
  [ time(1:3) peak(1:3) ]
  if peak(1) <= peak(3) | peak(3) <= peak(2)
    disp( ' WARNING - larger peaks needed' ) ;
	return;
  end
%
%  determine the steady state values
%
  yss = input( 'Enter initial time zero value ? ' );
  yinf = ( peak(1) * peak(3) - peak(2) * peak(2) ) / ...
     ( peak(1) + peak(3) - 2 * peak(2) );
  disp( [ '  Final time infinity value = ' num2str(yinf) ] ) ;
  yinf = input( 'Enter final time infinity value ? ' );
%
%----------------------------------------------------------------
%
%  estimate closed loop model parameters
%
cl_par = zeros(1,4) ;
dr = ( ( yinf - peak(2) ) / ( peak(1) - yinf ) ...
         + ( peak(3) - yinf ) / ( yinf - peak(2) ) ) / 2 ;
cl_par(3) = - log(dr) / sqrt( pi * pi + log(dr) * log(dr) ) ;
per = time(3) - time(1) ;
cl_par(2) = per * sqrt( 1 - cl_par(3) * cl_par(3) ) / 2 / pi ;
to = 0 ;
cl_par(4) = 2 * time(1) - time(2) - to ;
if cl_par(4) < 0
  cl_par(4) = 0 ;
end
setpt = input( 'Enter setpoint after step change ? ' ) ;
cl_par(1) = abs(yinf - yss) / abs(setpt - yss) ;
%
%  plot model
%
%  num = cl_par(1) ;
%  den = [ cl_par(2)*cl_par(2) 2*cl_par(2)*cl_par(3) 1 ] ;
%  tc = linspace( to, 4*time(3), 200 ) ;
%  yc = yss + (setpt - yss) * step( num, den, tc ) ;
%  clg
%  plot( tc, yc ) ;
%  title( 'IDENTIFIED CLOSED LOOP MODEL' ) ;
%  xlabel( 'Time' ) ;
%  text( 0.50, 0.35, 'Gain =', 'sc' ) ;
%  text( 0.65, 0.35, num2str(cl_par(1)), 'sc' ) ;
%  text( 0.50, 0.30, 'Tau  =', 'sc' ) ;
%  text( 0.65, 0.30, num2str(cl_par(2)), 'sc' ) ;
%  text( 0.50, 0.25, 'Zeta =', 'sc' ) ;
%  text( 0.65, 0.25, num2str(cl_par(3)), 'sc' ) ;
%  text( 0.50, 0.20, 'Dead =', 'sc' ) ;
%  text( 0.65, 0.20, num2str(cl_par(4)), 'sc' ) ;
%   pause ;
    disp( [' 1.  Gain = ' num2str(cl_par(1))] ) ;
    disp( [' 2.  Tau  = ' num2str(cl_par(2))] ) ;
    disp( [' 3.  Zeta = ' num2str(cl_par(3))] ) ;
    disp( [' 4.  Dead = ' num2str(cl_par(4))] ) ;
%
%-----------------------------------------------------------------
%
%  open loop model
%
kc = input( 'Controller gain for test ? ' ) ;
if abs( cl_par(1) - 1 ) > 0.02
  model = 1 ;
%
% if gain is not 1 then a FOPDT model
%
  ol_par = zeros(1,3) ;
  ol_par(1) = cl_par(1) / ( kc - cl_par(1) * kc ) ;
  ol_par(2) = 2 * cl_par(2) * cl_par(3) * ( 1 + abs(kc) * cl_par(1) ) ... 
                   - cl_par(4) * ( 1 - abs(kc) * cl_par(1) ) / 2 ;
  ol_par(3) =  2 * cl_par(4) ;
else
  model = 2 ;
%
% gain of 1 so an integrator plus FOPDT
%
  ol_par = zeros(1,3) ;
  ol_par(1) = sign(kc) / ( 2 * cl_par(2) * cl_par(3) + cl_par(4) ) ;
  ol_par(2) = cl_par(2) * cl_par(2) / ( 2 * cl_par(2) * cl_par(3) + cl_par(4) ) ;
  ol_par(3) = cl_par(4) ;
end
%
%  imc tuning
%
tf = ol_par(2) ;
ok = 0 ;
while ~ok
  disp( ' ' ) ;
  disp( [' Open loop parameters for model type ' num2str(model)] ) ;
  ol_par
  if model == 1
% PI    
    imc_pi = zeros(1,2) ;
    imc_pi(1) = ol_par(2) / ol_par(1) / ( tf + ol_par(3) ) ;
    imc_pi(2) = ol_par(2) ;
    imc_pi
% PID
    imc_pid = zeros(1,3) ;
    imc_pid(1) = ( ol_par(2) + ol_par(3) / 2 ) / ol_par(1) / ( tf + ol_par(3) / 2 ) ;
    imc_pid(2) = ol_par(2) + ol_par(3) / 2 ;
    imc_pid(3) = ol_par(2) * ol_par(3) / ( 2 * ol_par(2) + ol_par(3) ) ;
    imc_pid
  else
% PI
    imc_pi = zeros(1,2) ;
    imc_pi(1) = ( 2 * tf + ol_par(2) + ol_par(3) / 2 ) / ol_par(1) / ...
                                          ( tf + 3 * ol_par(3) / 2 ) ^ 2 ;
    imc_pi(2) = 2 * tf + ol_par(2) + ol_par(3) / 2 ;
    imc_pi
%  PID
    imc_pid = zeros(1,3) ;
    imc_pid(1) = ( 2 * tf + ol_par(2) + ol_par(3) ) / ol_par(1) / ( tf + ol_par(3) ) ^ 2 ;
    imc_pid(2) = 2 * tf + ol_par(2) + ol_par(3) ;
     imc_pid(3) = ( 2 * tf + ol_par(3) ) * ol_par(2) / ( 2 * tf + ol_par(2) + ol_par(3) ) ;
    imc_pid
  end
  ok = input( 'Closed loop response satisfactory (1=yes,0=no) ? ' ) ;
  if ~ok
    disp( [' Filter time constant = ' num2str(tf)] ) ;
    tf = input( 'New value ? ' ) ;
     end
  end
end
%
