%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%	UPDATED APRIL 15, 2010, Tara Chalermsongsak
%    linearize the Simulink block diagram of	 the     %	
%	      FREQUENCY STABILIZATION SERVO       	 % 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


% This gets the linearized model from the simulink model
% "psl5.mdl"
deg = 180/pi;

Modulation_Frequency = 35.5e6;                   % FSS f_sb in Hz



%+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
% Plant Parameters
%+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

Ref_Cav_Pole = 35e3;                             % Hz
Reference_Cavity = tf(2*pi*Ref_Cav_Pole,...
                     [1 2*pi*Ref_Cav_Pole]);
                 

                
%% Analyzed cavity added, PMC removed                
Analyzer_Cavity = tf(2*pi*Ref_Cav_Pole,...
                     [1 2*pi*Ref_Cav_Pole]);

Photodiode_Pole = 15e6;                          % New Focus 1811
Photodiode = tf(2*pi*Photodiode_Pole,...
               [1 2*pi*Photodiode_Pole]);

OpticalGain = 0.02e-7;                           % Watts per Hz
Efficiency = 0.8;                                % Amps per Watt
Zrf = 40000;                                     % Tuned Circuit Impedance
Grf = 1;                                         % Pre amplifier gain
DemodGain = 1/sqrt(2)/2;

VoltsPerHz = OpticalGain * Efficiency *...
             Zrf * Grf * DemodGain;
         
%PiezoActuatorGain = 5e6;                         % Hz per Volt

%PCActuatorGain = 0.001;                         % Radians per Volt


%=====================
% Acav path
%=====================
PDH_box = 1; % this will be replaced later with the measured TF
           
VCO=1;   %Hz/V
AOM=2;  % Hz/Hz





%+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
%   Ref Path, the signal goes through D980536-D
%+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
%==============
% D980536-D
% The calculation on Mar 31, 2010 in lab book
%==============


%========================
%signal from PD , ref cav
%========================

%=====================
%elliptic filter 12MHz
%=====================

r=50 ;
L1= 1.2 *10^-6;
C3= 200*10^-12;
C4= 47 *10^-12;
C5= 200*10^-12;
R8= 100;
R12= 200;



z1= r*tf(1 ,[C3*r  1]);
z2= tf([L1 0],[L1*C4 0 1]);
z3= tf(R8, [C5*R8  1]);
z4 = z2+z3;



Elliptic_Filter = z1*z3/(z1+z4);
        % the filter is specified as 12MHz Ellip, but is seems a bit higher 

   
CommonGain1 = R12/R8;
            % This will be adjustable gain before the signal is sent to  
            % Fast and PC paths


%++++++++++++++++++++



%====================
%Fast path, PZT, (to laser)
%=====================


R51= 1.1 *10^3;
R54=2.43*10^3;
C55 = 3300 *10^-12;

Fast_Gain1=1; %Adj gain 

Fast1 = (-R54/R51)*tf( 1/(C55*R54), [1 1/(C55*R54)] );


R50= 750;
R53= 1.58*10^3;
C54 = 0.01 *10^-6;


Fast2 = (-R53/R50)*tf( 1/(C54*R53), [1 1/(C54*R53)] );


R49= 3.74 *10^3;
R52=15.8 *10^3;

R55= 3.74 *10^3;
L2= 3.3*10^-3; 
C46 = 470 *10^-12;
C47 = 120 *10^-12;
Cs = C46 + C47;
R48 = 200;

zz = tf( [L2 R48 1/Cs],[1 0] ); % this is a  wire before Fast3

Fast3 = -R52/(R49 +R55 + R55*R49/zz);

Fast_Filter =  Fast_Gain1*Fast1*Fast2*Fast3 ; %total gain from PZT path

%====================
%PC path,very fast
%====================
%%
R17= 1.1 *10^3;
R14= 24.9 *10^3;
R22= 1.21 *10^3;
R23= 150;
R27= 15 *10^3;
R28= 1 *10^3;
R31= 100 *10^3;
R32= 20 *10^3;
R43= 10 *10^3;
R34= 100 *10^3;

C13= 47 *10^-9;
C18 = 2200 *10^-12;
C23 = 10 *10^-9;


PC1 = (-R14/R17)*zpk(0,-1/(C13*R17),1)...
          *tf( 1/(C18*R14), [1 1/(C18*R14) ]  );
% PC1 = (R14/(R17 +1/iwC13 ))* (1/ iwC18R14)
%This will be corrected, there's another part of 200pF and 100 ohm that I
%left out

PC2 = (- R27 / R22 )*...
         zpk(-1/(C23*R23), -1/(C23*(R23 + R27)) ,1);

 PC12   =  PC1*PC2;     

PC3 =   -(R32/R28)*( (PC12 + (R28/R31))/PC12 ); 
            %tf befrom V before R43 / V before R28
 
z13= tf(1, [1/C13  0] )     ;       
            

PC123 = (R32/R31)* ( PC12*(z13 +R31) + R28  )/...
                     ( PC12*z13 + R28 );
            %tf from PC 1,2 and 3 before C13 and after R32    
            %This can be used for TF (Vo/Vi) between TP7 and TP3.
            
            



PC4 = R34/R43;

PC_Filter= PC123*PC4;  % total gain from pc path

%++++++++++++
%++++++++++++
%++++++++++++




laser_act = 1  ;%Hz/V;    %%This will be measured

phase_act = 1   ;%rad/V    %% This will be measured











% On-Screen Parameters
%CommonGainSlider = -10;
%CommonGain = 10^(CommonGainSlider/20);

%FastGainSlider = 9;
% FastGain = 10^(FastGainSlider/20);

% Makes the State-Space object from the .mdl
[A,B,C,D] = linmod('psl5');
warning off
fss_sys = ss(A,B,C,D);