%
% ---------------------------------------------------------------
%  ACCDEMO.M is a script file that demonstrates the designs of 
%     1990 ACC benchmark problem.
% ---------------------------------------------------------------
%

% R. Y. Chiang & M. G. Safonov 
% Copyright (c) 1988 by the MathWorks, Inc.
% All Rights Reserved.
clc
clear
disp(' ')
disp('                      << Benchmark Problem >>')
disp('  ');
disp('                           |--> x1        |--> x2 = z (measurement)');
disp('         (control)    ------     k   ------');
disp('           u -------> | M1 | -/\/\/\-| M2 | ------> w (disturbance)');
disp('                      ------         ------');
disp('                       0  0           0  0');
disp('   ===============================================================');
disp('                     [ Design Specifications ]');
disp('    ------------------------------------------------------------');
disp('    | Design | Robustness   | Settling Time | Disturbance w(t) |')
disp('    ---------+--------------+---------------+-------------------');
disp('    |   1    | stable for   | Ts ~= 15 sec  |     impulse      |');
disp('    |        | .5 < k < 2   | for nominal   |                  |');
disp('    ---------+--------------+---------------+-------------------');
disp('    |   2    | maximize MSM | Ts ~= 15 sec  |     impulse      |');
disp('    |        | for nominal  | for nominal   |                  |');
disp('    ---------+--------------+---------------+-------------------');
disp('    |   3    | stable for   | Ts ~= 20 sec  | A sin(.5t + phi) |');
disp('    |        | .5 < k < 2   | for all cases | A, phi unknown   |');
disp('    ------------------------------------------------------------');
disp('   ')
disp(' ')
flag = input('Design # 1, 2 or 3 : ');
Lag = 0;   % Lag = 1; for design # 1, non-min. phase controller
if flag == 1
   Lag = input('  Controller type (0 = min phase, 1 = non-min phase): ')
end;
spring = [1 0.5 2];
%
clc
disp(' ')
disp('  -----------------------------------------------------------')
disp('    1. Set up the nominal system:');
disp(' ');
disp('       k = 1; m1 = 1; m2 = 1;');
disp('       ag = [0      0     1     0');
disp('             0      0     0     1');
disp('          -k/m1   k/m1    0     0');
disp('           k/m2  -k/m2    0     0];');
disp('       bg = [0 0 1/m1 0]`; cg = [0 1 0 0]; dg = 0;');
disp('  -----------------------------------------------------------')
k = 1; m1 = 1; m2 = 1;
ag = [0      0     1     0
      0      0     0     1
   -k/m1   k/m1    0     0
    k/m2  -k/m2    0     0];
bg = [0 0 1/m1 0]'; cg = [0 1 0 0]; dg = 0;
disp(' ');
disp('       Poles of G(s) - -')
lamg = eig(ag)
disp(' ')
disp('                       (strike a key to continue)')
pause
%---------------------------------------
% Augment Plant for H-Inf Design:
%
clc
if Lag == 0
  if flag == 1 | flag == 3 
      disp(' ')
      disp('  -----------------------------------------------------------');
      disp('    2. Set up the augment plant for H-Inf design:');
      disp(' ')
      disp('       Change the spring to "5/4" with scaling factor "Gam"')
      disp('       (the additive uncertainty is now bounded by +- 1)')
      disp('       k0 = 5/4;')
      disp('       A = [   0       0     1     0')
      disp('               0       0     0     1')
      disp('          -k0/m1   k0/m1     0     0')
      disp('           k0/m2  -k0/m2     0     0];')
      disp('       B1 = [0 0 -1/m1  1/m2]`; B2 = -[0 0 1/m1 0]`;')
      disp('       C1 = Gam*[1 -1 0 0];   C2 = [0 1 0 0];')
      disp('       D11 = 0; D12 = 0; D21 = 0; D22 = 0;')
      disp('       where "Gam" is the robustness level of spring')
      disp('       (Gam = 0.75 for 0.5 <= k <= 2).')
      disp('  ----------------------------------------------------------');
      disp('  ')
%      itcha = input('Assign iteration channel: (1=robustness, 2=control, 4=fix no. on each) ');
      itcha = 4;
      if itcha == 4
         disp(' ')
         if flag == 1
            disp(' Try:  Gam = 0.75, Rho = 0.02...')
         end
         if flag == 2
            disp(' Try:  Gam = 0.75, Rho = 0...')
         end
         if flag == 3
            disp(' Try:  Gam = 0.4,  Rho = 0.01...')
         end
         disp(' ')
         Gam = input('Assign the fix robustness level "Gam" of spring constant: ');
         Rho = input('Assign the fix bound "Rho" on control signal: '); 
      end
      if itcha == 2 
         Gam = input('Assign the fix robustness level "Gam" of spring constant: ');
         Rho = 1;
      end
      if itcha == 1 
         Rho = input('Assign the fix bound "Rho" on control signal: ');
         Gam = 1;
      end
      k0 = 1.25;
      A  = [   0       0     1     0
               0       0     0     1
          -k0/m1   k0/m1     0     0
           k0/m2  -k0/m2     0     0]; 
      B1 = [0 0 -1/m1  1/m2]'; B2 = -[0 0 1/m1 0]';
      C1 = Gam*[1 -1 0 0];   C2 = [0 1 0 0];
      D11 = 0; D12 = 0; D21 = 0; D22 = 0;
      if Rho ~= 0
          C1 = [C1;0 0 0 0]; D11 = [D11;0]; D12 = [D12;Rho];
      end
  end
end
if flag == 3
   clc
   disp(' ')
   disp('  ----------------------------------------------------------');
   disp('       Augment the plant with Internal Model:')
   disp('                         (s+1)^2')
   disp('           	  I(s) = -----------')
   disp('                        s^2 + 0.5^2')
   disp('  ----------------------------------------------------------');   
   [aint,bint,cint,dint] = tf2ss([1 2 1],[1 0 0.25]);
  if Lag == 0
   [aa,bb,cc,dd] = series(aint,bint,cint,dint,A,B2,[C1;C2],[D12;D22]);
   A = aa; B1 = [zeros(size(aint)*[1;0],1);B1]; B2 = bb;
   C1 = cc(1:2,:); C2 = cc(3,:); D12 = dd(1:2,1); D22 = dd(3,1);
  else
   [ag,bg,cg,dg] = series(aint,bint,cint,dint,ag,bg,cg,dg);
  end
   disp('  ')
   disp('  ')
   disp('                    (strike a key to continue)')
   pause   
end
if flag == 2 
   disp(' ')
   disp('  -----------------------------------------------------------');
   disp('    2. Set up the augment plant for H-Inf design:');
   disp(' ');
   disp('       A = ag; ');
   disp('       B1 = [0 0 0;0 0 0;-1/m1 1 0; 1/m2 0 1];')
   disp('       B2 = -[0 0 1/m1 0]`; ')
   disp('       C1 = [1 -1 0 0;-k k 0 0;k -k 0 0]; C2 = [0 1 0 0];')
   disp('       D11 = [0 0 0;-1 0 0;1 0 0]; D12 = -[0 1 0]`;')
   disp('       D21 = [0 0 0]; D22 = 0;')
   disp('  ----------------------------------------------------------'); 
   A = ag; B1 = [0 0 0;0 0 0;-1/m1 1 0; 1/m2 0 1]; B2 = -[0 0 1/m1 0]'; 
   C1 = [1 -1 0 0;-k k 0 0;k -k 0 0]; C2 = [0 1 0 0];
   D11 = [0 0 0;-1 0 0;1 0 0]; D12 = -[0 1 0]'; 
   D21 = [0 0 0]; D22 = 0;
   itcha = 1:3;   % iterate on 3 robustness channels
   disp('  ')
   disp('  ')
   disp('                    (strike a key to continue)')
   pause
end
%
clc
disp(' ')
disp(' ------------------------------------------------------------------')
disp('    3. Transform the Plant via Pole-Shifting Bilinear Transform:')
disp(' ')
disp('       % select the circle point for mapping ...')
disp('       % pack the 2-port state-space ...')
disp('       B = [B1 B2]; C = [C1;C2]; D = [D11 D12;D21 D22];')
disp('       [aa,bb,cc,dd] = bilin(A,B,C,D,1,`Sft_jw`,cirpt);')
disp('       % split the (aa,..) back to 2-port state-space (A,B1,..);')
disp('       % The H-Inf problem is ready to go ..')
disp(' ------------------------------------------------------------------')
disp(' ');
disp(' ');
if Lag == 0
   disp('                  (strike a key to see an example of bilinear mapping)')
   pause
   bilexp
else
   disp('                  (strike a key to continue..)')
   pause
end
disp(' ');
disp(' ');
disp('A class of H-Inf controllers can be found by iterating the circle point "p1":')
disp(' ')
disp(' ')
if flag == 1
   if Lag == 1
      disp('            (p2 = INF; Try p1 = -0.3)');
   else
      disp('            (Min. Phase Controller: p1 = -0.35)');
   end
end
if flag == 2
      disp('            (Min. Phase Controller: p1 = -0.465)'); 
      disp('            (Non-Min. Phase Controller: p1 = -0.25)');
end
if flag == 3
      disp('            (Min. Phase Controller: p1 = -0.3)');
end
cirpt1 = input('   Input the circle point "p1": ');
cirpt = [-100 cirpt1];
%
if Lag == 1
   sgm = -cirpt1; itcha = 4;
   [tmp1,tmp2]=size(ag);
   ag = ag + sgm*eye(tmp1,tmp2);
   disp(' ')
   disp('Only impose W2 weighting on control signal - -')
   Rho = input('   Assign the fix bound on W2 weight (try 0.1): '); 
   w1 = []; w3 = []; w2 = [Rho;1];
   [A,B1,B2,C1,C2,D11,D12,D21,D22] = augtf(ag,bg,cg,dg,w1,w2,w3);
end
no_u1 = size(B1)*[0;1]; no_u2 = size(B2)*[0;1];
no_y1 = size(C1)*[1;0]; no_y2 = size(C2)*[1;0];
if Lag == 0 
   B = [B1 B2]; C = [C1;C2]; D = [D11 D12;D21 D22];
   [aa,bb,cc,dd] = bilin(A,B,C,D,1,'Sft_jw',cirpt);
   A = aa; B1 = bb(:,1:no_u1); B2 = bb(:,no_u1+1:no_u1+no_u2); 
   C1 = cc(1:no_y1,:); C2 = cc(no_y1+1:no_y1+no_y2,:);
   D11 = dd(1:no_y1,1:no_u1); D12 = dd(1:no_y1,no_u1+1:no_u1+no_u2); 
   D21 = dd(no_y1+1:no_y1+no_y2,1:no_u1); 
   D22 = dd(no_y1+1:no_y1+no_y2,no_u1+1:no_u1+no_u2);
end
%
clc
disp(' ')
disp('  -------------------------------------------------------------')
disp('    4. Starting H-Inf Design: ');
disp(' ')
disp('       TSS_ = mksys(A,B1,B2,C1,C2,D11,D12,D21,D22,`tss`);')
if itcha == 4
      disp('       % Regular H-Inf ')
      disp('       [ss_cp,ss_cl,hinfo]=hinf(TSS_);')
else
      disp('       % H-Inf GAMMA-Iteration for maximum MSM')
      disp('       [gamopt,ss_cp,ss_cl]=hinfopt(TSS_);')
end
disp('       [acp,bcp,ccp,dcp] = branch(ss_cp);')
disp('       [acl,bcl,ccl,dcl] = branch(ss_cl);')
disp('  -------------------------------------------------------------')
disp('  ')
disp('  ')
disp('                    (strike a key to continue)')
pause
format short e
clc
if itcha == 4
      [acp,bcp,ccp,dcp,acl,bcl,ccl,dcl,hinfo] = hinf(...
                              A,B1,B2,C1,C2,D11,D12,D21,D22);
else
      [gamopt,acp,bcp,ccp,dcp,acl,bcl,ccl,dcl] = hinfopt(...
                              A,B1,B2,C1,C2,D11,D12,D21,D22,itcha);
end
disp('  ')
disp('                    (strike a key to continue)')
pause
%
acceva
accplt
%
% ------------ End of ACCDEMO.M % RYC/MGS %