%PID_joint Simulation of PID position control for a flexible joint
%
% FIRST, setup your A, B, and C matrices based on model parameters. Then 
% execute
%
%     [num,den] = ss2tf( A, B, C, [0; 0; 0] );
%
% which should give you three "num" rows and one "den" row.
%
%  *) The single "den" row corresponds to the s-domain polynomial
%     coefficients of the denominator that's shared among all of the
%     transfer functions (i.e., all transfer functions have the same
%     POLES because they're from the same system).
%
%  *) The three "num" rows correspond to the s-domain polynomial
%     coefficients for each of the three output transfer functions. Each
%     row of the transfer function corresponds to the output from each
%     row of the C matrix. Remember that in the time domain
%
%       y = C*x + D*u
%
%     where x is the state vector and y is the output vector. That is,
%     y(1) is the first output, y(2) is the second output, and y(3) is
%     the third output. We care about transfer functions Y(1)/U, Y(2)/U,
%     and Y(3)/U.
%
% So
%
%     tf( num(1,:), den(:) ) is the 1st transfer function ( Y(1)/U ),
%     tf( num(2,:), den(:) ) is the 2nd transfer function ( Y(2)/U ), and
%     tf( num(3,:), den(:) ) is the 3rd transfer function ( Y(3)/U ).
%
% So assign the num and den rows to Theta, Alpha, ThetaPlusAlpha, and 
% Vin so that
%
%     tf( Theta, Vin ) is Theta/Vin,
%     tf( Alpha, Vin ) is Alpha/Vin, and
%     tf( ThetaPlusAlpha, Vin ) is (Theta+Alpha)/Vin.
%
% Clearly U = Vin. It is your job to figure out how to assign Y(1),
% Y(2), and Y(3). OF COURSE, YOU MAY NEGLECT ENTRIES OF [num,den] THAT
% ARE VERY CLOSE TO ZERO, **BUT** VERIFY THAT THEY ARE EQUAL TO ZERO BY
% INSPECTING THEM DIRECTLY (e.g., type num(2,1) to view that entry).
% DON'T BE FOOLED BY MATLAB'S CHOICE OF DISPLAY *SCALE*.
%
% Finally, set your Kp and Kd gains (leave Ki=0). Every time you run the 
% script, you will get plots comparing the "lab" and "ideal" outputs and 
% controls. Adjust Kp and Kd (again, leave Ki=0) to meet book 
% specifications:
%
%    *) Less than 0.5% steady-state error
%    *) Peak overshoot less than 2%
%    *) Settling time (to within 2% of steady-state) less than 0.6 seconds
%
% Keep the +/- 5V saturation in mind when setting Kp. Start with Ki=0. 
% Keep Kp very small (e.g., 1 or 2 orders of magnitude smaller than Kp).
%
% REMEMBER TO TRY IMPLEMENTING SOLUTIONS FOR *BOTH*
%
%   Rm = 2.6 ohms (book model)
%
% AND
%
%   Rm = 3.9 ohms (possibly more realistic class model)

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

% ***** MAKE SURE WORKSPACE VARIABLES ARE SETUP BY THIS POINT!!! ******
% 
% It's a good idea to modify this script so that it sets up your 
% workspace variables *for you* automatically.
%
% I would have done this for you, but I wanted to leave some things up 
% to you. :)
%
% See the instructions above (or type "help PID_joint" in the command
% window) for information on how to get your workspace variables setup
% using ss2tf.
%
% *********************************************************************

% Give students the option of specifying a Ki if needed
if( exist('Ki','var') )
    % Found a Ki. Use it!
    
    % PIDcontroller is used in step
    PIDcontroller = tf( [Kd Kp Ki], [1 Ki] );
else
    % No Ki found. Set to zero.
    
    Ki = 0;    
    
    % PIDcontroller is used in step (with Ki=0)
    PIDcontroller = tf( [Kd Kp], [1] ); % = tf( [Kd Kp 0], [1 0] );
end

% The "sim" command runs the Simulink model that you 
% pass to it (as a string)
sim( 'PID_joint_ideal' );       % Ideal model with "smooth" Simulink PID
sim( 'PID_joint_lab' );         % Lab-based model with nonlinear effects

% Also calculate what we'd expect for a "true" PID controller (if it could
% exist)
totalOLsys = PIDcontroller*tf( ThetaPlusAlpha, Vin );
total_fantasy = step( totalOLsys/(1+totalOLsys), t );

% Do the same for alpha
alphaOLsys = PIDcontroller*tf( Alpha, Vin );
alpha_fantasy = step( alphaOLsys/(1+alphaOLsys), t );

% Clear out anything from before and save the time NOW
clf
nowstring = datestr( now );

% First, plot the total angular position
outputaxis = subplot(311);
plot( t, total, '-', ...
      t_lab, total_lab, '--', ...
      t, total_fantasy, '-.', ...
      t_lab, ones(size(t_lab)), 'k:' );
legend( 'Simulink PID \theta+\alpha  Total Response', ...
        'Lab PID \theta+\alpha  Total Response', ...
        'RLTool (Fantasy PID) \theta+\alpha  Total Response', ...
        'Unit Step', ...
        'Location', 'SouthEast' );
xlabel( 'Time (s)' );
ylabel( 'Total Angle \theta+\alpha (radians)' );
title( [ 'Joint Sim of Total (\theta+\alpha ): ' ...
         'K_p = ' num2str(Kp), ', ', ...
         'K_i = ' num2str(Ki), ', ', ...
         'K_d = ' num2str(Kd), ...
         ' @ ' nowstring ] );

% Next, plot the tip deflection
alphaaxis = subplot(312);
plot( t, alpha, '-', ...
      t_lab, alpha_lab, '--', ...
      t, alpha_fantasy, '-.', ...
      t_lab, zeros(size(t_lab)), 'k:' );
legend( 'Simulink PID \alpha  Tip Response', ...
        'Lab PID \alpha  Tip Response', ...
        'RLTool (Fantasy PID) \alpha  Tip Response', ...
        'Location', 'SouthEast' );
xlabel( 'Time (s)' );
ylabel( 'Tip Deflection \alpha (radians)' );
title( [ 'Joint Sim of Tip (\alpha ): ' ...
         'K_p = ' num2str(Kp), ', ', ...
         'K_i = ' num2str(Ki), ', ', ...
         'K_d = ' num2str(Kd), ...
         ' @ ' nowstring ] );     
     
% Finally, plot the control effort
controlaxis = subplot(313);
plot( t, u, '-', t_lab, u_lab, '--', t_lab, zeros(size(t_lab)), 'k:' );
legend( 'Simulink PID Control', ...
        'Lab PID Control', ...
        'Location', 'NorthEast' );
xlabel( 'Time (s)' );
ylabel( 'Control Effort (V)' );
title( [ 'Joint Sim of Control (u): ' ...
         'K_p = ' num2str(Kp), ', ', ...
         'K_i = ' num2str(Ki), ', ', ...
         'K_d = ' num2str(Kd), ...
         ' @ ' nowstring ] );     

% Link the axes so that zooming in on x in one does so in the others too
linkaxes( [ outputaxis alphaaxis controlaxis ], 'x' );

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

%
% 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        ~
