% steering_place.m - vehicle steering by state feedback
% kja 070327

aminit;					% MATLAB initialization

% Parameter definitions (roughly a human on a Segway)
Tf=10;
a=0.5;

% System dynamics
A=[0 1;0 0];
B=[a;1];
C=[1 0];
zc=0.7;

% Perform designs for different values of wc 
% larger values more agressive control
figure(1); clf; 
for wc=[0.5 1 2]
  K=[wc^2 2*zc*wc-a*wc^2];        %Feedback gains
  Ac=A-B*K;                       %Closed loop dynamics matrix
  kr=-1/(C*(Ac\B))                %Feedforward gain kr=wc^2;
  Bc=kr*B;
  syscl=ss(Ac,Bc,C,0);            %Step response
  [y,t,x]=step(syscl,Tf);

  subplot(321); hold on;
  plot(t, y, 'b', 'LineWidth', AM_data_linewidth);
  hold on; %plot([0 16], [1 1], 'k', 'LineWidth', AM_ref_linewidth);

  amaxis([0 Tf 0 1.3]);
  ylabel('Position y');

  subplot(323); hold on;
  u=kr-K*x';                      %Control signal
  plot(t, u, 'Linewidth', AM_data_linewidth);

  amaxis([0 Tf -1 4]);
  xlabel('Time (sec)'); ylabel('Steer angle');
  hold on;
end

% Add arcs showing direction of parameter change
subplot(321); arcarrow([2.8 0.3], [0.1 0.9], 8); amtext(3, 0.3, 'wc');
subplot(323); arcarrow([0.15 -0.3], [0.85 1.7], -12); amtext(1.1, 1.7, 'wc');

% Print the final plot
amprint('steering-place1.eps');

%Now fix wc and investigate effect of damping zc
wc=1;
figure(2); clf; 
for zc=[0.5 0.7 1.0],
  K=[wc^2 2*zc*wc-a*wc^2];
  Ac=A-B*K;
  kr=-1/(C*(Ac\B));
  Bc=kr*B;
  syscl=ss(Ac,Bc,C,0);
  [y,t,x]=step(syscl,Tf);

  subplot(322); hold on;
  plot(t, y, 'Linewidth', AM_data_linewidth);

  amaxis([0 Tf 0 1.3]);
  ylabel('Position y');

  subplot(324); hold on;
  u=kr-K*x';
  plot(t, u, 'Linewidth', AM_data_linewidth);

  amaxis([0 Tf -0.5 1]);
  xlabel('Time (sec)'); ylabel('Steer angle');
  hold on;
end    

% Add arcs showing direction of parameter change
subplot(322); arcarrow([1 1], [2.7 0.6], -8); amtext(2.9, 0.6, 'zc');
subplot(324); arcarrow([2 -0.45], [2.5 0.09], -12); amtext(2.7, 0.2, 'zc');

amprint('steering-place2.eps');
