
% Torsional Mode Damping for the GAS Filter Chain

% -------------------------------------------------------------------- %
% This program calculate yaw motions of the filters in SAS.
% The magnet disc is suspended from the filter zero and damps
% the yaw motions of the filters by eddy current.
% A transfer function and impulse response (from torque exerted to the
% payload, to torsional angle of the payload) are plotted.
% -------------------------------------------------------------------- %
% (This program requires Control System Toolbox software.)
% -------------------------------------------------------------------- %
%   Editor: T.Sekiguchi
% �@2010/11/30  First release
%   2010/12/16  Add the control box
%   2011/01/10  Eliminate the filter zero
% -------------------------------------------------------------------- %

% -------------------------------------------------------------------- %
% Set Parameter
% -------------------------------------------------------------------- %
  
  clear all;
  
  % Stiffness [N m/rad]
  k1 = 0.70; % >= 0.70
  k2 = 0.70; % >= 0.50
  k3 = 0.70; % >= 0.34
  k4 = 0.50; % >= 0.20
  k5 = 0.27; % >= 0.27
  k6 = 18.0;
  
  % Inertia Moment [kg m^2]
  If = 6.24;  % GAS Filter:  estimated by Gianni-san
  Ic = 4.0;   % Control Box: not fixed (2.0, 4.0 or 6.0)
  Ip = 5.61;  % Payload:     1.95 + 2.35 + 0.69 + 0.45 + 0.17
  Im = 1.84;  % Magnet Disc: with 70cm diameter & 30kg mass
  
  % Damping Coefficient [N m/rad]
  Eddy = 5;
  
% -------------------------------------------------------------------- %
% Cosntruct State-Space Model
% -------------------------------------------------------------------- %
  
  % State-Space Model Matrix
  a71 = -(k1+k2)/If;
  a72 =       k2/If;
  a77 =    -Eddy/If;
  a7C =     Eddy/If;
  a81 =       k2/If;
  a82 = -(k2+k3)/If;
  a83 =       k3/If;
  a92 =       k3/If;
  a93 = -(k3+k4)/If;
  a94 =       k4/If;
  aA3 =       k4/Ic;
  aA4 = -(k4+k5)/Ic;
  aA5 =       k5/Ic;
  aB4 =       k5/Ip;
  aB5 =      -k5/Ip;
  aC6 =      -k6/Im;
  aC7 =     Eddy/Im;
  aCC =    -Eddy/Im;
  b71 =        1/If;
  b82 =        1/If;
  b93 =        1/If;
  bA4 =        1/Im;
  bB5 =        1/Ip;
  
  Amdl = [...
     %  1   2   3   4   5   6   7   8   9   A   B   C 
        0   0   0   0   0   0   1   0   0   0   0   0 ; % 1
        0   0   0   0   0   0   0   1   0   0   0   0 ; % 2
        0   0   0   0   0   0   0   0   1   0   0   0 ; % 3
        0   0   0   0   0   0   0   0   0   1   0   0 ; % 4
        0   0   0   0   0   0   0   0   0   0   1   0 ; % 5
        0   0   0   0   0   0   0   0   0   0   0   1 ; % 6
       a71 a72  0   0   0   0  a77  0   0   0   0  a7C; % 7
       a81 a82 a83  0   0   0   0   0   0   0   0   0 ; % 8
        0  a92 a93 a94  0   0   0   0   0   0   0   0 ; % 9
        0   0  aA3 aA4 aA5  0   0   0   0   0   0   0 ; % A
        0   0   0  aB4 aB5  0   0   0   0   0   0   0 ; % B
        0   0   0   0   0  aC6 aC7  0   0   0   0  aCC];% C
  Bmdl = [...
     %  1   2   3   4   5
        0   0   0   0   0 ; % 1
        0   0   0   0   0 ; % 2
        0   0   0   0   0 ; % 3
        0   0   0   0   0 ; % 4
        0   0   0   0   0 ; % 5
        0   0   0   0   0 ; % 6
       b71  0   0   0   0 ; % 7
        0  b82  0   0   0 ; % 8
        0   0  b93  0   0 ; % 9
        0   0   0  bA4  0 ; % A
        0   0   0   0  bB5; % B
        0   0   0   0   0]; % C
     
       %  1 2 3 4 5 6 7 8 9 A B C
  Cmdl = [0 0 0 0 1 0 0 0 0 0 0 0];
  
  Dmdl = 0;
  
  mdl = ss(Amdl,Bmdl,Cmdl,Dmdl);
  
% -------------------------------------------------------------------- %
% Bode Plot
% -------------------------------------------------------------------- %

  figure(101);
  impulseplot(mdl(5))
  title('Impulse Response','FontSize',16,'FontWeight','bold')
  xlabel('Time','FontSize',14,'FontWeight','bold')
  ylabel('Amplitude','FontSize',14,'FontWeight','bold')
  set(gca,'FontSize',14)
  set(gca,'XColor',[0 0 0])
  set(gca,'YColor',[0 0 0])
  
  fmin = -2.5; fmax = -0.5;
  magmin = -1.5; magmax = 3;
  freqVec = logspace(fmin,fmax,5000);
  
  [mag phs1] = bode(mdl(5),freqVec*(2*pi));
  mag = squeeze(mag);
  phs1= squeeze(phs1); abs = exp(1i*phs1*pi/180);
  phs = angle(abs)*180/pi;

  figure(201);
  subplot(5,1,[1 2 3])
  loglog(freqVec,mag,'LineWidth',2)
  grid on
  ylim([10^(magmin) 10^(magmax)])
  xlim([10^(fmin) 10^(fmax)])
  title('System Bode Plot','FontSize',16,'FontWeight','bold')
  ylabel('Magnitude (m/N)','FontSize',14,'FontWeight','bold')
  set(gca,'FontSize',14)
  
  subplot(5,1,[4 5])
  semilogx(freqVec,phs,'LineWidth',2)
  grid on
  ylim([-180 180])
  xlim([10^(fmin) 10^(fmax)])
  ylabel('Phase (deg)','FontSize',14,'FontWeight','bold')
  xlabel('Frequency (Hz)','FontSize',14,'FontWeight','bold')
  set(gca,'FontSize',14)
  set(gca,'YTick',-180:90:180)



    