%
%  Matlab driver for the for PID control of a simple wheel
%
% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  Model Parameters
%
  MAX_ISUM = 10000;
  MAX_DELTA_U = 50000;
  MAX_DUTY = 2*7300;
  AD_scale = 1.0;
  scale = MAX_DUTY/1023.0;
%
%  read the parameters from the file
%       
   data = load('Step_Response_kp');
   t = data(:,1);
   r = data(:,2);
   u_max = data(:,3)*AD_scale/scale;
   isum = data(:,5);
   rt = [t r];
   y = data(:,4);
   Ts = t(3)-t(2);
   Tf = t(end);
%
%  Compute the limiting parameters
%   
  Relay_On = 0;   % minimum positive control effort to make the motor start spinning in rad/sec
  Relay_Off = 0;  % motor stops at control efforts below this in rad/sec
  Max_Speed = 200.0;  % maximum motor speed in rad/sec
  Control_Effort_Saturation = MAX_DUTY*AD_scale/scale % maximum allowable control effort
  
  Rising_Slew = MAX_DELTA_U*AD_scale/(Ts*scale);  % maximum positive slope for control effort
  Falling_Slew = -MAX_DELTA_U*AD_scale/(Ts*scale);  % maximum negative slope for control effort
%
%  enter the discrete-time model parameters (ignore delays)
%
  B = 1.0;
  b = 0.5;
%
%  enter the transfer function
%  
  D = tf(1,[1 0 0 0],Ts);  % enter the delay here as a transfer function
%  D = tf(1,1, Ts);
  Gp = tf(B*[1],[1 -b], Ts)*D
  [num_Gp, den_Gp] = tfdata(Gp,'v');

%
%  enter the prefilter value
%
  Gpf = 1.0;
%
%  enter the PID parameters
%
  kp = 5.0;
  ki = 0.0;
  kd = 0.0;
%
%  PI Controller
%
%  K = 0.206;
%  a = -0.887;
%  kp = -K*a;
%  ki = K-kp;
%
%  PID Controller
%
%   K = 22.024;
%   a = -1.88;
%   b = 0.884;
%   kd = K*b
%   kp = -K*a-2*kd
%   ki = K-kp-kd
%
%  now simulate the system
%
  sim('DT_PID');
%
%  plot the results
%
  figure
  orient landscape

%
%  Use the following when comparing to the measured data
%
  subplot(2,1,1);
  hold on
  title(['G_{pf} = ' num2str(Gpf,4) ', k_p = ' num2str(kp,4) ', k_i = ' num2str(ki,4) ', k_d = ' num2str(kd,4)]);
  [mts, yts] = stairs(m_time,m_y); 
  [ts, ys] = stairs(t, y);
  [tr, yr] = stairs(t, r);
  plot(ts,ys,'r',mts,yts,'b',tr,yr,'k','Linewidth',1.5);
  legend('Measured','Model','Reference'); grid; xlabel('Time (sec)'); ylabel('Speed (rad/sec)');
  hold off
  subplot(2,1,2); 
  [mts, mtu] = stairs(m_time,m_u); 
  [ts, us] = stairs(t, u_max);
  plot(ts,us,'r',mts,mtu,'b','Linewidth',1.5); legend('Measured','Model'); grid; xlabel('Time (sec)'); ylabel('Control Effort (rad/sec)');
%   subplot(3,1,3); 
%   [mts, mis] = stairs(m_time,m_isum); 
%   [ts, sis] = stairs(t,isum);
%   plot(ts,sis,'r',mts,mis,'b','Linewidth',1.5); legend('Measured','Model'); grid; xlabel('Time (sec)'); ylabel('ISUM (rad/sec)');
