%
%  Matlab driver for the basic 1-dof state variable model
%  This version is for a PID controller
%
%  set up the matrices for constructing the states
%  this is for the ECP Simulink program
%
  get_positions = [1 0 0 0 0; 
                   0 1 0 0 0; 
                   0 0 1 0 0];
  get_angles = [0 0 0 1 0];
  get_error_bit = [0 0 0 0 1];
  get_all_states = [1 0 0 0 0 0 0 0; 
                    0 0 0 1 0 0 0 0; 
                    0 1 0 0 0 0 0 0; 
                    0 0 0 0 1 0 0 0; 
                    0 0 1 0 0 0 0 0; 
                    0 0 0 0 0 1 0 0; 
                    0 0 0 0 0 0 1 0; 
                    0 0 0 0 0 0 0 1];
 %
 % this is the only part you should ever have to change
 %
 get_desired_states = [1 0 0 0 0 0 0 0; 
                       0 1 0 0 0 0 0 0];
% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
  saturation_level = 1000/2196; % set the limiter for the model 210
  saturation_level = 1000/2546; % set the limiter for the model 205
%
%  load the continuous time state model
%
  load bobs_1dof_205
  Hc = ss(A,B,C,D);
%
%  determine the sampling interval
%
  Ts = 0.1;
%  
%  discretize the state space model, then get
%  the state variable model back
%
  Hd = c2d(Hc,Ts,'zoh');
  [GG,HH,CC,DD] = ssdata(Hd);
%
%  Now construct the discrete-time state model with the delay
%
  Gc = [GG HH; zeros(1,3)];
  Hc = [zeros(2,1); 1];
  Cc = eye(3,3);
  Dc = 0;
%
  G = Gc; H = Hc; C = Cc; D = Dc;
  C = [1 0 0];
%
%  get the discrete-time transfer function
%
  [numGp, denGp] = ss2tf(G, H, C, D )
  Gp = tf(numGp, denGp, Ts)
%
%  eliminate coefficients that are only due to roundoff
%
  tol = 1e-10;
  numGp = (abs(numGp)>tol).*numGp;
  denGp = (abs(denGp)>tol).*denGp;
  Gp = tf(numGp, denGp, Ts)
%
%  set the PI parameters....
%
%    read in parameters from sisotool 
%
  K = 0.0116;
  a = -1.0;
  b = 0.0;
%
%    convert to PI parameters
%
  Kp = -K*a;
  Ki = K-Kp;
  Kd = 0.0
%
%    convert to PID parameters
%
%  Kd = 
%  Kp =
%  Ki =
%
%  set the prefilter gain
%
  Gpf = 1.0;
%  
%  set the step amplitude and final time
%
  Amp = 1.0; % desired final position, in cm
  Amp = 15*pi/180; % desired final position, in rad

  Tf = 3.0;  % seconds
%
%  now simulate the system
%
  sim('DT_PID');
%
%  plot the results
%
  figure
  orient landscape % plot sideways
  scale = 180/pi; 
  %scale = 1.0;
  subplot(2,1,1); stairs(m_time,m_y*scale); grid; xlabel('Time (sec)'); ylabel('Position (deg)');
  subplot(2,1,2); stairs(m_time,m_u); grid; xlabel('Time (sec)'); ylabel('Control Effort');
  axis([0 Tf -saturation_level saturation_level]);
