%Robustness analysis of cruise control
% kja 060712
aminit;

%m=1600;k0=0.2755;ki0=0.06916;K=[];
m=1600;k0=0.7151;ki0=0.1814;K=[];R=[];

% Clear previous traces
figure(1); clf;

% Set up the pole zero plot
figure(2); clf; xsubplot(221); hold on;
ampolezero(tf([1], [1]), [-1.25 0.5 -0.75 0.75]);

% Iterate over the car mass
for m=[1600 2000]
  % Reset values to their defaults
  wm=420;Tm=190;beta=0.4;rho=1.3;Cd=0.32;A=2.5;Cr=0.01;g=9.8;

  % Iterate over the center of mass location (?)
  for al=[16 12 10];

    % Iterate over the velocity
    for v=[10 40];
      % Compute out the parameter values    
      theta=0;
      F=al*Tm*(1-beta*((al*v/wm)-1)^2);
      Fd=m*g*Cr+rho*A*Cd*v^2/2+m*g*theta;
      u0=Fd/F;
      T=u0*F;
      Tp=-2*u0*al^2*Tm*beta*(al*v/wm-1)/wm;
      a=(Tp-rho*Cd*A*v)/m;
      b=F/m;

      %Simulation of PI cruise control
      t=0:0.1:20;
      z=1;
      w0=0.5;
      k=k0;ki=ki0;

      %for w0=[0.2 0.5 1]
      %k=(2*z*w0-a)/b;
      %ki=w0^2/b;
      rr=roots([1 -a+b*k b*ki]);
      R=[R;rr];
      K=[K;[a b k ki]];
      Ge=0.05*tf(9.8*[1 0],[1 -a+b*k b*ki]);
      Gu=0.05*tf(9.8*[k ki],[1 -a+b*k b*ki]);
      [e,t]=step(Ge,t);
      [u,t]=step(Gu,t);
      
      % Plot the step response
      figure(1); subplot(421); hold on;
      pl=plot(t, e, 'Linewidth', AM_thin_linewidth);

      % Plot the input response
      subplot(423); hold on;
      pl=plot(t, u+u0, 'Linewidth', AM_thin_linewidth);
    end
  end
  
  %Plot closed loop poles for robust cruise control
  % kja 060712
  %Run cruiseconrobcon first to get the matrix R
  figure(2);
  plot(real(R), imag(R), 'o', 'MarkerSize', AM_data_markersize);
end

% Step responses  
figure(1); 
subplot(421); amaxis; box on;
xlabel('Time {\itt} [s]');
ylabel('Error {\ite}');

subplot(423); box on;
xlabel('Time {\itt} [s]');
ylabel('Input {\itu}');

amprint('cruise-robtime.eps');

figure(2); amaxis;
plot([-1 -0.5 0], [0 0 0], 'k+', 'LineWidth', AM_tick_linewidth);
plot([0 0 0], [-0.5 0 0.5], 'k+', 'LineWidth', AM_tick_linewidth);
amtext(-1.05, -0.1, '-1'); amtext(-0.575, -0.1, '-0.5');
amtext(0.07, -0.5, '-0.5'); amtext(0.07, 0.5, '0.5');

amprint('cruise-robeigs.eps');