scdleadlagnotch(blockname,TunableParameters)

function S = scdleadlagnotch(blockname,TunableParameters) 
%---------------------------------------------
%|     The Digital Motion Control Demo       |
%---------------------------------------------
%| A practical lesson in Model-Based Design. |
%| Prepared by:                              |
%| Paul Lambrechts, March, 2007              |
%| Copyright 2007, The MathWorks, Inc.       |
%---------------------------------------------
%| Note: This demo is prepared with R2006b   |
%---------------------------------------------
% Based on
% SCDLEADEXAMPLE  Configuration function for the lead-lag controller demo.
%
% SCDLEADEXAMPLE creates the configuration structure S which is used by
% Simulink Control Design to register a masked subsystem for compensator
% design.  The configuration function is called with the name of the block,
% blockname and a structure vector containing the evaluated block mask
% variables.

% Author(s): John W. Glass 18-Jul-2005
% Copyright 2005 The MathWorks, Inc.
% $Revision: 1.1.6.2 $ $Date: 2006/02/02 03:06:52 $
                         
%% Set up the evaluation function.  This is the function that converts the
%  block dialog parameters in TunableParameters to a zero/pole/gain form
%  for control design.
EvalFcn = @LocalEvalFcn;

%% Set up the inverse function.  This is the function that converts the
%  zero/pole/gain form of the compensator to its corresponding block
%  parameters.
InvFcn = @LocalInvFcn;

%% Create the constraints.  In this case the compensator can have a maximum
%  of 1 pole and 1 zero.  The static gain is freely tunable.
Constraints = struct('MaxZeros',3,'MaxPoles',3,...
                     'isStaticGainTunable',true);

%% Register the parameters 
S = struct('TunableParameters',TunableParameters,...
           'EvalFcn',EvalFcn,...
           'InvFcn',InvFcn,...
           'Constraints',Constraints,...
           'Inport',1,...
           'Outport',1);

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% LocalEvalFcn computes the tunable component C and the fixed component
% Cfixed of the compensator.  
function [C,Cfixed] = LocalEvalFcn(TunableParameters)

%% Specify the zero/pole/gain data given the parameter values 
k          = TunableParameters(1).Value;
leadzero   = TunableParameters(2).Value;
lagpole    = TunableParameters(3).Value;
notchzero  = TunableParameters(4).Value;
zerodamp   = TunableParameters(5).Value;
notchpole  = TunableParameters(6).Value;
poledamp   = TunableParameters(7).Value;

%% Check input sizes
if ~isscalar(k) || ~isscalar(leadzero) || ~isscalar(lagpole) || ...
        ~isscalar(notchzero) || ~isscalar(zerodamp) || ...
        ~isscalar(notchpole) || ~isscalar(poledamp) 
    error('The block parameters for the lead-lag-notch block must be scalars.')
end

%% Check for the case where parameters are zero
if (k == 0) || (leadzero == 0) || (lagpole == 0) ||...
        (notchzero == 0) || (zerodamp == 0) ||...
        (notchpole == 0) || (poledamp == 0)
    error('The block parameter for the lead-lag-notch block must be non-zero');
end

%% Check for the case where the notch parameters are empty or infinity
if isempty(notchzero) || isempty(zerodamp) || ...
   isempty(notchpole) || isempty(poledamp) || ...
   ~isfinite(notchzero) || ~isfinite(zerodamp) || ...
   ~isfinite(notchpole) || ~isfinite(poledamp)
    error('The notch parameters for the lead-lag-notch block must be defined and finite');
end

%% Compute the tune component and handle the cases where there are no
%  poles or zeros.  If a zero or pole is empty it will have a value of
%  infinity.
if ~isempty(lagpole) && ~isempty(leadzero) && (isfinite(lagpole) && isfinite(leadzero))
    C1 = zpk(-leadzero*2*pi,-lagpole*2*pi,k*lagpole/leadzero);
elseif isempty(leadzero) && ~isempty(lagpole) || (isfinite(lagpole) && ~isfinite(leadzero))
    C1 = zpk([],-lagpole*2*pi,k*lagpole*2*pi);
elseif isempty(lagpole) && ~isempty(leadzero) || (~isfinite(lagpole) && isfinite(leadzero))
    C1 = zpk(-leadzero*2*pi,[],k/(leadzero*2*pi));
else
    C1 = zpk([],[],k);
end
% Now the notch:
if (notchzero == notchpole) && (zerodamp == poledamp)
    C2 = zpk([],[],1);
else
    C2 = tf([1/(2*pi*notchzero)^2 2*zerodamp/(2*pi*notchzero) 1],...
            [1/(2*pi*notchpole)^2 2*poledamp/(2*pi*notchpole) 1]);
    C2 = zpk(C2);
end
C = C1 * C2;

%% The fixed element is a gain of 1
Cfixed = zpk([],[],1);

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% LocalEvalFcn computes the block dialog parameters given pole-zero-gain 
% values. 
function TunableParameters_out = LocalInvFcn(TunableParameters_in,z,p,k)

%% Initialize the output parameters
TunableParameters_out = TunableParameters_in;

%% Find the real pole and zero
leadzero=[];
lagpole=[];
for i=1:length(z)
    if isreal(z(i)), leadzero=z(i)/2/pi; zreal=i; end
    if isreal(p(i)), lagpole=p(i)/2/pi;  preal=i; end
end
% Remove real pole and zero from input
z(zreal)=[];
p(preal)=[];

%% Find the notch
if length(z) ~= 2 || length(p) ~= 2
    notchzero = 1; zerodamp = 1;
    notchpole = 1; poledamp = 1;
else
    C = zpk([],p,1);
    [notchpole,poledamp]=damp(C);
    C = zpk([],z,1);
    [notchzero,zerodamp]=damp(C);
end    
knotch=notchpole(1)^2/notchzero(1)^2;

%% Compute the inverse and handle the cases where the poles and zeros are
%  deleted.
if ~isempty(leadzero) && ~isempty(lagpole)
    %% 1 zero and 1 pole
    TunableParameters_out(1).Value = k*leadzero/lagpole/knotch;
    TunableParameters_out(2).Value = -leadzero;
    TunableParameters_out(3).Value = -lagpole;
elseif isempty(z) && ~isempty(p)
    %% 1 pole and no zero.  Assign a value of inf for the zero.
    TunableParameters_out(1).Value = -k/lagpole/knotch;
    TunableParameters_out(2).Value = inf;
    TunableParameters_out(3).Value = -lagpole;
elseif ~isempty(z) && isempty(p)
    %% 1 zero and no pole.  Assign a value of inf for the pole.
    TunableParameters_out(1).Value = -k*leadzero/knotch;
    TunableParameters_out(2).Value = -leadzero;
    TunableParameters_out(3).Value = inf;
else
    %% No zeros or poles.  In this case the block becomes a gain.
    TunableParameters_out(1).Value = k;
    TunableParameters_out(2).Value = inf;
    TunableParameters_out(3).Value = inf;
end

%% Now the notch
    TunableParameters_out(4).Value = notchzero(1)/2/pi;
    TunableParameters_out(5).Value = zerodamp(1);
    TunableParameters_out(6).Value = notchpole(1)/2/pi;
    TunableParameters_out(7).Value = poledamp(1);