%% SETUP_SRV02_EXP03_SPD
%
% Sets the necessary parameters to run the SRV02 Experiment #3: Speed
% Control laboratory using the "s_srv02_spd" and "q_srv02_spd" Simulink 
% diagrams.
% 
% Copyright (C) 2010 Quanser Consulting Inc.
%
clear all;
%
%% SRV02 Configuration
% External Gear Configuration: set to 'HIGH' or 'LOW'
EXT_GEAR_CONFIG = 'HIGH';
% Encoder Type: set to 'E' or 'EHR'
ENCODER_TYPE = 'E';
% Is SRV02 equipped with Tachometer? (i.e. option T): set to 'YES' or 'NO'
TACH_OPTION = 'YES';
% Type of Load: set to 'NONE', 'DISC', or 'BAR'
LOAD_TYPE = 'DISC';
% Amplifier Gain used: set to 1, 3, or 5
K_AMP = 1;
% Power Amplifier Type: set to 'UPM_1503', 'UPM_2405', 'VoltPaq', or 'Q3'
AMP_TYPE = 'VoltPaq';
% Digital-to-Analog Maximum Voltage (V)
VMAX_DAC = 10;
%
%% Lab Configuration
% Type of controller: set it to 'AUTO', 'MANUAL'
% CONTROL_TYPE = 'AUTO';
 CONTROL_TYPE = 'MANUAL';
%
%% PI Specifications
if strcmp(AMP_TYPE,'Q3')
    % Peak time (s)
    tp = 0.20;
    % Percentage overshoot (%)
    PO = 10.0;
    % Steady-state error (rad)
    e_ss_des = 0.0;
else
    % Peak time (s)
    tp = 0.05;
    % Percentage overshoot (%)
    PO = 5;
    % Steady-state error (rad)
    e_ss_des = 0.0;
end
%
%% Control Specifications of Compensated Open-Loop System
if strcmp(AMP_TYPE,'Q3')
    % Desired crossover frequency (rad/s)
    wg_des = 30.0;
    % Desired phase margin (deg)
    PM_des = 75.0;
else
    % Desired crossover frequency (rad/s)
    wg_des = 75.0;
    % Desired phase margin (deg)
    PM_des = 75.0;
end
%
%% SRV02 System Parameters
% Set Model Variables Accordingly to the USER-DEFINED SRV02 System Configuration
% Also Calculate the SRV02 Model Parameters and 
% Write them to the MATLAB Workspace (to be used in Simulink diagrams)
[ Rm, kt, km, Kg, eta_g, Beq, Jm, Jeq, eta_m, K_POT, K_TACH, K_ENC, VMAX_AMP, IMAX_AMP ] = config_srv02( EXT_GEAR_CONFIG, ENCODER_TYPE, TACH_OPTION, AMP_TYPE, LOAD_TYPE );
%
%% Filter Parameters
% Encoder high-pass filter used to compute velocity
% Cutoff frequency (rad/s)
wcf_e = 2 * pi * 63.0;
wcf = 2 * pi * 50.0;
% Damping ratio
zetaf_e = 0.9;
zetaf = 0.9;
%
%% Calculate Control Gains
if strcmp ( CONTROL_TYPE , 'MANUAL' )
    % Load model parameters based on SRV02 configuration.
    [ K, tau ] = d_model_param(Rm, kt, km, Kg, eta_g, Beq, Jeq, eta_m, AMP_TYPE);
    % Proportional gain (V/rad)
    kp = 0;
    % Integral gain (V/rad/s)
    ki = 0;
    % Set-point weight
    bsp = 1;
    % Lead Compensator
    a = 1;
    T = 1;
    % Lead proportional gain (V/rad/s)
    Kc = 0;
elseif strcmp ( CONTROL_TYPE , 'AUTO' )
    % Load model parameters based on SRV02 configuration.
    [ K, tau ] = d_model_param(Rm, kt, km, Kg, eta_g, Beq, Jeq, eta_m, AMP_TYPE);
    % Calculate PI control gains given specifications.
    [ kp, ki ] = d_pi_design( K, tau, PO, tp, AMP_TYPE );
    % Set-point weight
    bsp = 0;
    % Lead Compensator
    if strcmp(AMP_TYPE,'Q3')    
        d_lag_q3;
    else
        d_lead;
    end
end
%
%% Display
if strcmp(AMP_TYPE,'Q3')    
    disp( ' ' );
    disp( 'SRV02 model parameters: ' );
    disp( [ '   K = ' num2str( K, 3 ) ' rad/s^2/A' ] );
    disp( 'PI control gains: ' );
    disp( [ '   kp = ' num2str( kp, 3 ) ' A/rad' ] );
    disp( [ '   ki = ' num2str( ki, 3 ) ' A/rad/s' ] );
    disp( 'Lag compensator parameters: ' );
    disp( [ '   Kc = ' num2str( Kc, 3 ) ' A.s/rad' ] );
    disp( [ '   1/T = ' num2str( 1/T, 3 ) ' rad/s' ] );
    disp( [ '   1/(a*T) = ' num2str( 1/(a*T), 3 ) ' rad/s' ] );
else
    disp( ' ' );
    disp( 'SRV02 model parameters: ' );
    disp( [ '   K = ' num2str( K, 3 ) ' rad/s/V' ] );
    disp( [ '   tau = ' num2str( tau, 3 ) ' s' ] );
    disp( 'PI control gains: ' );
    disp( [ '   kp = ' num2str( kp, 3 ) ' V/rad' ] );
    disp( [ '   ki = ' num2str( ki, 3 ) ' V/rad/s' ] );
    disp( 'Lead compensator parameters: ' );
    disp( [ '   Kc = ' num2str( Kc, 3 ) ' V/rad/s' ] );
    disp( [ '   1/(a*T) = ' num2str( 1/(a*T), 3 ) ' rad/s' ] );
    disp( [ '   1/T = ' num2str( 1/T, 3 ) ' rad/s' ] );

end
