function [fastdata, slowdata] = PID
%PID Simulation of PID controller approximations.
%   Compares several PID controller approximations (use different
%   derivative approximations)
%
%   [fastdata, slowdata] = PID
%       fastdata is a structure with data using book system
%       slowdata is a structure with data using modified (slow) system
%
%   fast = PID
%       Calling PID with only one output argument causes processing only 
%       to occur for fast system.
%
%   PID
%       Calling PID with no output arguments is identical to calling it 
%       with two except that it doesn't return anything.
%
%   NOTE: THIS SCRIPT REQUIRES THAT THE Simulink MODEL PIDsim.mdl IS
%         AVAILABLE!!

%
% Copyright (c) 2008 by Theodore P. Pavlic
%
% This work is licensed under the Creative Commons
% Attribution-Noncommercial 3.0 United States License. To view a copy of
% this license, visit http://creativecommons.org/licenses/by-nc/3.0/us/
% or send a letter to Creative Commons, 171 Second Street, Suite 300,
% San Francisco, California, 94105, USA.
%

% Upper-case    A B C D E F G H I J K L M N O P Q R S T U V W X Y Z
% Lower-case    a b c d e f g h i j k l m n o p q r s t u v w x y z
% Digits        0 1 2 3 4 5 6 7 8 9
% Exclamation   !           Double quote "          Hash (number) #
% Dollar        $           Percent      %          Ampersand     &
% Acute accent  '           Left paren   (          Right paren   )
% Asterisk      *           Plus         +          Comma         ,
% Minus         -           Point        .          Solidus       /
% Colon         :           Semicolon    ;          Less than     <
% Equals        =           Greater than >          Question mark ?
% At            @           Left bracket [          Backslash     \
% Right bracket ]           Circumflex   ^          Underscore    _
% Grave accent  `           Left brace   {          Vertical bar  |
% Right brace   }           Tilde        ~

%%%%%%%%%%%%%%%%%%%%%%%%%%% Start of Script %%%%%%%%%%%%%%%%%%%%%%%%%%%

% Use N=200 in the lab for derivative filtering/approximation
N = 200;

% Use 5V saturation threshold for plant
saturation_threshold = 5;

% Use 0.1V dead zone threshold for plant
% (good idea to make dead_zone_threshold < saturation_threshold)
% (otherwise, integrator has a hard time getting ahead of dead zone)
dead_zone_threshold = 0.1;

% Use 1000 units/s slew rate
slew_rate = 1000;

% LSB from "Analog-to-Digital" Converter (shaft encoder)
ADCLSB = 2*pi/2^10;

% LSB from "Digital-to-Analog" converter (output quantization)
% (+/-10V, 16-bit DAC on RTI1104 cards)
DACLSB = 2*10/2^16;

%% Setup for fast system
plant_num = [ 1 ];
plant_den = [ 0.0026 0.1081 0 ];

% Setup PID controller 
% (these gains are nice for theoretical PID but poor for real/lab)
Kd = 1;
Ki = 50^2*Kd;
Kp = 100/sqrt(2)*Kd;
% (here is a manually tuned solution (no error, no overshoot, "fastish"))
%Kd = 0.075; Ki = 0.5; Kp = 5;

figure(1);
if nargout > 0
    fastdata = do_sim( plant_num, plant_den, Kp, Ki, Kd, ...
                       N, ...
                       saturation_threshold, dead_zone_threshold, ...
                       slew_rate, DACLSB, ADCLSB );
else
    do_sim( plant_num, plant_den, Kp, Ki, Kd, ...
            N, saturation_threshold, dead_zone_threshold, ...
            slew_rate, DACLSB, ADCLSB );
end
    
subplot(211); 
title( [ 'Fast System (' datestr(now) ')' ], ...
       'Interpreter', 'latex', 'FontSize', 14 );


%% Setup for slow system
plant_num = [ 1 ];
plant_den = [ 0.0039 0.1081 0 ];

% Setup PID controller (again, theoretical gains that are poor in lab)
Kd = 1.5;
Ki = 50^2*Kd;
Kp = 100/sqrt(2)*Kd;
%Kd = 0.075; Ki = 0.5; Kp = 5;

% Only bother with slow system processing if we have zero or 2 outputs
if mod(nargout,2) == 0
    figure(2);
    
    if nargout == 2
        slowdata = do_sim( plant_num, plant_den, Kp, Ki, Kd, ...
                           N, ...
                           saturation_threshold, dead_zone_threshold, ...
                           slew_rate, DACLSB, ADCLSB );
    elseif nargout == 0
        do_sim( plant_num, plant_den, Kp, Ki, Kd, ...
                N, saturation_threshold, dead_zone_threshold, ...
                slew_rate, DACLSB, ADCLSB );
    end

    subplot(211);
    title( [ 'Slow System (' datestr(now) ')' ], ...
           'Interpreter', 'latex', 'FontSize', 14 );

end

%% Functions that do the simulations
    function [simdata] = do_sim( plant_num, plant_den, Kp, Ki, Kd, ...
                                 N, ...
                                 saturation_threshold, ...
                                 dead_zone_threshold, ...
                                 slew_rate, ...
                                 DACLSB, ADCLSB )
        %
        % plant_num and plant_den 
        %   are vectors of polynomial coefficients describing the numerator
        %   and denominator of plant transfer function
        %
        % Kp, Ki, and Kd 
        %   are the proportional, integral, and derivative gains
        %
        % N 
        %   is pole of differentiator approximation (s/(s/N+1))
        %   (note: N < Inf; defaults to 10000 rad/s)
        %
        % saturation_threshold 
        %   is clipping discontinuity (defaults to 5V)
        %
        % dead_zone_threshold
        %   is dead zone discontinuity (defaults to 0.1V)
        %
        % slew_rate
        %   is maximum rate of change of input (defaults to 1000 units/s)
        %
        % DACLSB
        %   is the input quantization threshold (defaults to 2*pi/1024)
        %
        % ADCLSB
        %   is the output quantization threshold (defaults to 2*10/2^(16))
        if nargin < 11
            ADCLSB = 2*10/2^(16);
            
        if nargin < 10
            DACLSB = 2*pi/1024;
            
            if nargin < 9
                slew_rate = 1000;

                if nargin < 8
                    dead_zone_threshold = 0.1;

                    if nargin < 7
                        saturation_threshold = 5;

                        if nargin < 6
                            N = 10000;
                        end

                    end
                end

            end
        
        end
        end
        
        % These statements let Simulink write to variables within the scope
        % of this function
        persistent r t
        persistent e_lab u_lab y_lab        
        persistent e u y
        persistent e_pida u_pida y_pida
        persistent e_manual u_manual y_manual
        persistent e_manuala u_manuala y_manuala
                    
        % Must copy signals to base workspace in order for Simulink to see
        % them
        assignin( 'base', 'plant_num', plant_num );
        assignin( 'base', 'plant_den', plant_den );
        assignin( 'base', 'Kp', Kp );
        assignin( 'base', 'Ki', Ki );
        assignin( 'base', 'Kd', Kd );
        assignin( 'base', 'N', N );
        assignin( 'base', 'saturation_threshold', saturation_threshold );
        assignin( 'base', 'dead_zone_threshold', dead_zone_threshold );        
        assignin( 'base', 'slew_rate', slew_rate );        
        assignin( 'base', 'DACLSB', DACLSB );        
        assignin( 'base', 'ADCLSB', ADCLSB );        
        
        % Run Simulink on model
        sim( 'PIDsim' );
        
        % Generate RLTOOL-type step responses 
        plant = tf( plant_num, plant_den );        
        
        % First, generate RLTOOL response for lab topology
        PIcontroller = tf( [Kp Ki], [1 0] );
        Gd = tf( [Kd 0], [1] );
        plantGd = plant/(1+Gd*plant);
        sysd = PIcontroller*plantGd;
        y_rltoollab = step( sysd/(1+sysd), t );
        e_rltoollab = step( 1/(1+sysd), t );
        u_rltoollab = ...
            step( PIcontroller/(1 + plant*(PIcontroller + Gd)), t );
        % note: PIDcontroller = PIcontroller + Gd

        % First, generate RLTOOL response for lab topology with filter
        PIcontroller = tf( [Kp Ki], [1 0] );
        Gdf = tf( [Kd 0], [1/N 1] );
        plantGdf = plant/(1+Gdf*plant);
        sysdf = PIcontroller*plantGdf;
        y_rltoollabf = step( sysdf/(1+sysdf), t );
        e_rltoollabf = step( 1/(1+sysdf), t );
        u_rltoollabf = ...
            step( PIcontroller/(1 + plant*(PIcontroller + Gdf)), t );
        % note: filteredPIDcontroller = PIcontroller + Gdf        
        
        % Generate RLTOOL-type step response for PID
        %PIDcontroller = Kd*(s^2 + (Kp/Kd)*s + (Ki/Kd))/s;
        PIDcontroller = tf( [ Kd Kp Ki ], [1 0] );
        sys = PIDcontroller*plant;
        y_rltool = step( sys/(1+sys), t );
        e_rltool = step( 1/(1+sys), t );
        % u_rltool cannot be generated because r-to-u system has more zeros
        % than poles (i.e., it's not causal)
        %u_rltool = step( PIDcontroller/(1+sys), t );
        
        if( nargout > 0 )
            % Setup data to return
            simdata.t = t;
            simdata.r = r; 
            simdata.e_lab = e_lab;
                simdata.u_lab = u_lab;
                simdata.y_lab = y_lab;            
            simdata.e = e; 
                simdata.u = u; 
                simdata.y = y;
            simdata.e_pida = e_pida; 
                simdata.u_pida = u_pida; 
                simdata.y_pida = y_pida;
            simdata.e_manual = e_manual;
                simdata.u_manual = u_manual;
                simdata.y_manual = y_manual;
            simdata.e_manuala = e_manuala;
                simdata.u_manuala = u_manuala;
                simdata.y_manuala = y_manuala;
            simdata.e_rltoollab = e_rltoollab;
                simdata.u_rltoollab = u_rltoollab;
                simdata.y_rltoollab = y_rltoollab;
            simdata.e_rltoollabf = e_rltoollabf;
                simdata.u_rltoollabf = u_rltoollabf;
                simdata.y_rltoollabf = y_rltoollabf;                
            simdata.e_rltool = e_rltool;
                simdata.y_rltool = y_rltool;                
        end
            
        
        % Clear the previous figure stuff for plotting
        cla
        
        % Plot the output signals
        subplot(211);
        yfigs = plot( ...
            t, y_lab, '-.', ...
            t, y, 'm--', ...
            t, y_pida, ':', ...
            t, y_manual, 'm--', ...
            t, y_manuala, 'm--', ...
            t, y_rltoollab, '-', ...
            t, y_rltoollabf, '-.', ...            
            t, y_rltool, '-', ...            
            t, ones(size(t)), 'k--', ...
            t, 1.02*ones(size(t)), 'k--', ...
            t, 0.98*ones(size(t)), 'k--', ...
            t, 1.043*ones(size(t)), 'k--' );
        xlim([ 0 t(end) ]);
        ylim([ 0 1.6 ]);
        set( yfigs([1 3]), 'LineWidth', 1.2 );

        ylabel( 'Position Output ($y$)', ...
                'Interpreter', 'latex', 'FontSize', 14 );
        xlabel( 'Time (s)', 'Interpreter', 'latex', 'FontSize', 14 );
        lfig = legend( ...
            'Lab PID Implementation (most realistic)', ...            
            'Ideal Simulink PID', ...
            'Simulink PID Approximation', ...
            'Manual PID (identical to Ideal Simulink PID)', ...
            'Manual PID Approximation (broken)', ...
            'RLTOOL Lab PID (lab with no nonlinearities)', ...            
            'RLTOOL Lab Filtered PID (lab with no nonlinearities)', ...                        
            'RLTOOL PID (ideal with no nonlinearities)', ...            
            'Location', 'SouthEast' );
        set(lfig, 'Interpreter', 'latex', 'FontSize', 12);
        
        % Plot the control signals
        subplot(212);
        ufigs = plot( ...
            t, u_lab, '-.', ...
            t, u, 'm--', ...
            t, u_pida, ':', ...
            t, u_manual, 'm--', ...
            t, u_manuala, 'm--', ...
            t, u_rltoollab, '-', ...
            t, u_rltoollabf, '-.', ...            
            t, Kp*ones(size(t)), ...
            t, ( Kp + Kd*N )*ones(size(t)), ...
            t, zeros(size(t)), 'k--' );
        xlim([ 0 t(end) ]);
        %ylim( ceil( max(abs(u_pida))/50 )*50*[-1 1] );
        %ylim( [-Kp Kp+Kd*N] );
        ylim( ceil( max(abs(Kp))/50 )*50*[-1 1] );
        set( ufigs([1 3]), 'LineWidth', 1.2 );
        
        ylabel( 'Control Signal ($u$)', ...
                'Interpreter', 'latex', 'FontSize', 14 );
        xlabel( 'Time (s)', 'Interpreter', 'latex', 'FontSize', 14 );
        lfig = legend( ...
            [ 'Lab PID Implementation (max = ' ...
              num2str(max(abs(u_lab))) ')' ], ...              
            [ 'Ideal Simulink PID (max = ' ...
              num2str(max(abs(u))) ')' ], ...
            [ 'Simulink PID Approximation (max = ' ...
              num2str(max(abs(u_pida))) ')' ], ...
            [ 'Manual PID (max = ' ...
              num2str(max(abs(u_manual))) ')' ], ...
            [ 'Manual PID Approximation (max = ' ...
              num2str(max(abs(u_manuala))) ')' ], ...
            [ 'RLTOOL Lab PID (max = ' ...
              num2str(max(abs(u_rltoollab))) ')' ], ...
            [ 'RLTOOL Lab Filtered PID (max = ' ...
              num2str(max(abs(u_rltoollabf))) ')' ], ...                            
            [ '$K_p = ' num2str(Kp) '$' ], ...
            [ '$K_p + K_d N = ' num2str(Kp + Kd*N) '$' ], ...
            'Location', 'SouthEast' );
         set(lfig, 'Interpreter', 'latex', 'FontSize', 12);

         subplot(212);
         title( [ '$N = a = ' num2str(N) '$; ' ...
                  'Saturation forces $|u| <= ' ...
                    num2str(saturation_threshold) '$; ' ...
                  'Dead zone at $|u| <= ' ...
                    num2str(dead_zone_threshold) '$; ' ...
                  'Input slew rate is $' ...
                    num2str(slew_rate) '$ units$/$s' ], ...
                'Interpreter', 'latex', 'FontSize', 14);

         
    end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%% End of Script %%%%%%%%%%%%%%%%%%%%%%%%%%%%
