%
%  This function computes the controller for 
%  a zero velocity error ITAE system
%
%  Inputs:  Amp = amplitude of the step (in cm)
%           Gp = the plant transfer function
%           w0 = the desired natural frequency
%           order = the order of the closed loop system
%           final_time = final time to run the simulation for
%           filename = the name of the file containing the measured data
%
function ITAE_1(Amp,Gp,w0,order,final_time,filename);
%
%  Enter the desired closed loop transfer function
%
  if (order == 2 )
    a0 = w0^2;
    a1 = 3.2*w0;
    G0 = tf(a0,[1 a1 a0]);
  elseif (order == 3) 
    a0 = w0^3;
    a1 = 3.25*w0^2;
    a2 = 1.75*w0;
    G0 = tf(a0,[1 a2 a1 a0]);
  elseif (order == 4)
    a0 = w0^4;
    a1 = 5.14*w0^3;
    a2 = 4.93*w0^2;
    a3 = 2.41*w0;
    G0 = tf(a0,[1 a3 a2 a1 a0]);
  else
    fprintf(1,'Error, order must be between 2 and 4');
    return
  end;
 disp('The ITAE optimal transfer function ....');
  G0
%
%  Compute the closed loop transfer function
%
%  Gc = G0/Gp(1-G0) = N0*D/(D0*N-N*N0)
%
  [numG0,denG0] = tfdata(G0,'v');
  [numGp,denGp] = tfdata(Gp,'v');
  top = conv(numG0,denGp);
  bot = conv(denG0,numGp)-conv(numGp,numG0);
  Gc = tf(top,bot);
%
%  Now we have to remove pole/zero cancellations
%
  [numGc,denGc] = tfdata(Gc,'v');
%
%  first determine the gain, so we can fix it later
%
  [z,gain] = zero(Gc);
%
%  now remove the poles and zeros that cancel
%
  z = roots(numGc);
  p = roots(denGc);
  p_index = [];
  z_index = [];
  tol = 1e-8;
  for i = 1:length(z)
      for k = 1:length(p)
          if ( abs(z(i)-p(k)) < tol )
              p_index = [p_index k];
              z_index = [z_index i];
          end;
      end;
  end;
%
%  Now construct the numerator and denominator
%
  p_final = [];
  for k=1:length(p)
     ok = 1;
     for i=1:length(p_index)
       if(k == p_index(i)) ok = 0; end;
     end;
     if (ok == 1) 
         p_final = [p_final p(k)];
     end;
  end;
  z_final = [];
  for k=1:length(z)
     ok = 1;
     for i=1:length(z_index)
       if(k == z_index(i)) ok = 0; end;
     end;
     if (ok == 1) 
         z_final = [z_final z(k)];
     end;
  end;
%
%  Now get the numerator and denominator polynomials
%
 top = poly(z_final);
 bot = poly(p_final);
%
%  any coefficient below a tolerance is assumed to be zero
%
  tol = 1e-8;
  level = tol * ones(1,length(top));
  test = abs(top) > level;
  top = test .* top;
  level = tol * ones(1,length(bot));
  test = abs(bot) > level;
  bot = test .* bot;
%
%  Now adjust the gain
%
  Gc = tf(top,bot);
  [z,newgain] = zero(Gc);
  Gc = Gc*gain/newgain;
%
  [numGc,denGc] = tfdata(Gc,'v');
%
%  Now scale the coefficients
%
  scale = 1/max(denGc);
  num = scale*numGc;
  den = scale*denGc;
  disp('The controller transfer function ...');
  Gc = tf(num,den)  
  fprintf('\n');
  fprintf(['S = (0 to N) ' num2str(fliplr(num),5) '\n']);
  fprintf(['R = (0 to N) ' num2str(fliplr(den),5) '\n']); 
  fprintf('\n');
  G1 = feedback(Gc*Gp,1);
%
%  Now simulate the system
%
  t = linspace(0,final_time,1000);
  u = Amp*ones(1,length(t));
  y = lsim(G1,u,t);
%
%  plot the results
%
  if (~isempty(filename))
    orient landscape
    encoder = 1;
    data = importdata(filename);
    tdata = data(:,encoder+1);
    ydata = data(:,encoder+3)/2196;
    plot(t,y,':',tdata,ydata,'-'); grid;
    ymin = min(min(y),min(ydata));
    ymax = max(max(y),max(ydata));
    axis([0 final_time ymin ymax]);
    legend('Model y','Real System y',4);
    xlabel('Time (sec)');
    ylabel('Position (cm)');
    title(['ITAE e_v = 0, order = ' num2str(order) ', \omega_0 = ' num2str(w0)]);
  else    % no data to compare to
    orient landscape
    plot(t,y,'-'); grid;
    xlabel('Time (sec)');
    ylabel('Position (cm)');
    title(['ITAE e_v = 0, order = ' num2str(order) ', \omega_0 = ' num2str(w0)]);
    legend('Model x_1',4);
  end;
  %
%  Now spit out the coefficients for the ECP system
%
  fp = fopen('values.par','w');
  fprintf(fp,'[DYNFWD_C]\n');
%
%  get the numerator
%
  N =  length(num);
  temp = [zeros(1,8-N) num];
  S = fliplr(temp);
%
%  get the denominator
%
  N = length(den);
  temp = [zeros(1,8-N) den];
  R = fliplr(temp);
%
%  now write it out
%
  format long 
  fprintf(fp,'s0=%15.12e\n',S(1));
  fprintf(fp,'s1=%15.12e\n',S(2));
  fprintf(fp,'s2=%15.12e\n',S(3));
  fprintf(fp,'s3=%15.12e\n',S(4));
  fprintf(fp,'s4=%15.12e\n',S(5));
  fprintf(fp,'s5=%15.12e\n',S(6));
  fprintf(fp,'s6=%15.12e\n',S(7));
  fprintf(fp,'s7=%15.12e\n',S(8));
  fprintf(fp,'r0=%15.12e\n',R(1));
  fprintf(fp,'r1=%15.12e\n',R(2));
  fprintf(fp,'r2=%15.12e\n',R(3));
  fprintf(fp,'r3=%15.12e\n',R(4));
  fprintf(fp,'r4=%15.12e\n',R(5));
  fprintf(fp,'r5=%15.12e\n',R(6));
  fprintf(fp,'r6=%15.12e\n',R(7));
  fprintf(fp,'r7=%15.12e\n',R(8));
  fclose(fp);
%
  return;