%
%  Matlab driver for the basic 1-dof state variable model
%  This version is for the 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
%
%  create the discrete-time model
%
  load bobs_210_model
  Hc = ss(A,B,C,D);
%  
%  discretize the state space model, then get
%  the state variable model back
%
  Ts = 0.1;
  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 parameters
%
  Gpf = 1.0
%
% initial proportional controller
%
  Kp = 0.0116
  Ki = 0.0;
  Kd = 0.0;
%
%  PI Controller
%
  K = 0.0;
  a = 0.0;
%
%  Kp = -K*a;
%  Ki = K-Kp;
%  Kd = 0.0;
%
%  PID Controller
%
  K = 0.0;
  a = 0.0;
  b = 0.0;
%
% Kd = K*b;
% Kp = -K*a-2*Kd;
% Ki = K-Kp-Kd;
 
%  
%  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 = 2.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 (cm)');
  subplot(2,1,2); stairs(m_time,m_u); grid; xlabel('Time (sec)'); ylabel('Control Effort');
  axis([0 Tf -saturation_level saturation_level]);