%
%  Matlab driver for the for PID control of a pendulum
%
% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  Model Parameters
%
%
  MAX_ISUM = 0.4;
  z = 0.0511;
  wn = 9.3317;
  K = 1.329;
  Gp_continuous = tf(K*[1 0 0],[1 2*z*wn wn*wn]);
%
%  convert to discrete-time transfer function
%
  Ts = 0.0175;
  Delay = tf(1,[1 0 0],Ts);
  Gp_discrete = c2d(Gp_continuous, Ts)
%  Gp_discrete = Gp_discrete*Delay
%
 [num_Gp, den_Gp] = tfdata(Gp_discrete,'v')
%
%  set the initial condition (assuming 2nd order)
%
  phi_initial = 0.01;
  X1 = 0.0;
  X0 = phi_initial/(-num_Gp(1)*den_Gp(2)+num_Gp(2))
%
%  enter the final time
%
  Tf = 1.0;
%
%  enter the PID parameters
%
  kp = 0.0; % 
  ki = 0.5; %
  kd = 0.0;
%
%  now simulate the system
%
  sim('DT_PID');
%
%  plot the results
%
  figure
  orient landscape
  subplot(2,1,1);
  [mts, yts] = stairs(m_time,m_y); 
  plot(mts,yts,'b','Linewidth',1.5);
  grid; xlabel('Time (sec)'); ylabel('\phi (rad/sec)');
  title([' k_p = ' num2str(kp,4) ', k_i = ' num2str(ki,4) ', k_d = ' num2str(kd,4)]);
  
  subplot(2,1,2);
  [mts, mis] = stairs(m_time,m_isum); 
  plot(mts,mis,'b','Linewidth',1.5);
  grid; xlabel('Time (sec)'); ylabel('Isum (rad/sec)');