%  CLOSED LOOP FREQUENCY RESPONSE

%  FILENAME: BODECL.M
%  DATE: October 1994
%  VERSION: 02
%  COPYRIGHT 1993, 1994 M.Gretzinger & T.Marlin
%            McMaster University   

% This m-file is called from STABLE.M. It sets up a menu to enable the user
%       to specify the start and end frequency, set point or disturbance 
%       and plot the amplitude ratio of the transfer function, using the 
%       parameters specified by the process, disturbance and tuning data.
%       For the disturbance response, the user has the option to have the 
%       disturbance and process dynamics the same, or to specify unique 
%       disturbance dynamics in the disturbance menu.

while 1
  clc
  disp('*************************************************************')
  disp('*           CLOSED LOOP FREQUENCY RESPONSE                  *')
  disp('*                                                           *')
  disp('*************************************************************')
  disp('')
  disp('SELECT THE APPROPRIATE MENU ITEM')
  disp('')
  disp('MODIFY...')
  disp('                                    PRESENT VALUES')
  fprintf('1) Lowest Frequency                 %4.5g\n',wbegin)
  fprintf('2) Highest Frequency                %4.2f\n',wstop)
  if responsetype == 's'
      disp('3) Select Set Point or Disturbance    s')
  else
      disp('3) Select Set Point or Disturbance    d')
  end
  disp('4) Create Bode plot')
  disp('5) Return to main menu')
  disp('')
        
  m6 = input('Enter the desired selection:   ');
  
  if isempty(m6), break, end
      
  m6 = round(m6);
  if (m6<1) | (m6>5)
      disp('')
      disp('Not a valid selection')
      disp('')
      disp('Press ENTER to continue')
      pause;
  end    
  
  if m6 == 1
       junkwbegin = input('Enter value for lowest frequency:  ') ;
       if isempty(junkwbegin)
           junkwbegin = wbegin ;
       end
       if junkwbegin >= wstop
          junkwbegin = wbegin;
          disp(' illegal entry, > highest')
          disp(' press enter to continue')
          pause
       end
       wbegin = junkwbegin;


  elseif m6 == 2
       junkwstop = input('Enter value for highest frequency:  ') ;
       if isempty(junkwstop)
           junkwstop = wstop  ;
       end
       if junkwstop <= wbegin
           junkwstop = wstop;
           disp ('illegal entry, < lowest')
           disp ('press enter to continue')
           pause
       end
       wstop = junkwstop;

  elseif m6 == 3
       disp('For set point, enter the letter:    s')
       disp('For disturbance, enter the letter:  d')
       disp('')
       junktype = input('Selection:  ','s') ;
       if junktype ~= 's' & junktype ~= 'd'
          junktype = responsetype;
          disp (' illegal entry, please reenter ')
          disp (' press enter to continue')
          pause
       end
       if isempty(junktype)
           junktype = 'd' ;
       end
       responsetype = junktype;

  elseif m6 == 4

       if responsetype == 'd'
          answer = input('Are disturbance dynamics same as process dynamics? [y/n]  ','s');
          if answer ~= 'y' & answer ~= 'n'
             answer = 'y';
          end
          if answer == 'y'
             thetad = theta; taulead_d = taulead; 
             taud1 = tau1; taud2 = tau2; taud3 = tau3; xid = xi;
          elseif answer == 'n'
             thetad = 0; taulead_d = 0; 
             taud1 = taudist;  taud2 = 0; taud3 = 0; xid = 0;
          end
       end

       % Block to calculate transfer function (disturbance or set point), 
       % and amplitude ratio over the chosen frequency range. A vector of 
       % frequencies, called w, is defined using logspace. The vector of 
       % 500 frequencies are evenly spaced in the log domain between 
       % wbegin and wstop. Each element in the vector is then multiplied 
       % by jj to create the vector s. The vectors gp, gc and G 
       % are then calculated. Elementwise multiplication(.*), division(./) 
       % and powers(.^) are used in order that the expressions for gp, gc 
       % and G are carried out once for each element in the vectors making 
       % up the expressions.
           
       jj = sqrt(-1) ;
       w = logspace(log10(wbegin), log10(wstop), 500) ;
       s = jj*w ;
              
       gp = kp*exp(-theta*s).*(taulead*s + 1)...
             ./((tau1*s + 1).*(tau2*s + 1).*(tau3^2*s.^2 + 2*xi*tau3*s + 1));

       gd = kpd*exp(-thetad*s).*(taulead_d*s + 1)...
         ./((taud1*s + 1).*(taud2*s + 1).*(taud3^2*s.^2 + 2*xid*taud3*s + 1));
           
       % If block to prevent division by zero which would occur when
       % there is no integral mode.
       if ti == 0
          gc = kc*(1 + td*s) ;
       else
          gc = kc*(1 + (ti*s).^(-1) + td*s) ;
       end

      % If block to calculate the closed loop transfer function, 
      % depending on whether set point or disturbance is chosen.
       if responsetype == 's'
          G = gp.*gc./(1 + gp.*gc) ;
       elseif responsetype == 'd'
          G = gd./(1 + gp.*gc) ;
       end
           
      % Calculate the vector of amplitude ratios of G. The function ANGLE
      % calculates the amplitude ratios of each of the complex numbers 
      % in the vector G.
       ar = abs(G) ;
           
      % Create amplitude ratio plot
       clf ; axis('auto')
       loglog(w, ar)
       title('CLOSED-LOOP BODE PLOT')
       xlabel('Frequency, w  (rad/time)')
       ylabel('Amplitude Ratio')
       figure (1)
       pause

       close all
       clear w s gp gd gc G ar

  else
       break
  end        % if m6   (line )
  
end    % while 1 (line )