% invpend_phaseplot.m - phase plot for balanced pendulum
% RMM, 14 Jul 07

aminit;
a = 2;					% controller gain

figure(1); clf; 
subplot('position', [0.13, 0.6, 0.375 0.375]);
hold on;

% Sort out the region of attraction by saving simulations
[time, soln1] = ode45('invpend_bal', [0 3.0], [-5.94 5], [], a);
[time, soln2] = ode45('invpend_bal', [0 3.0], [2*pi -4.165], [], a);
[time, soln3] = ode45('invpend_bal', [0 3.0], [5.94 -5], [], a);
[time, soln4] = ode45('invpend_bal', [0 3.0], [-2*pi 4.165], [], a);

roa = [soln1; soln2(length(soln2):-1:1, :); ...
  soln3; soln4(length(soln4):-1:1, :)];
patch(roa(:,1), roa(:,2), [0.9 0.9 0.9]);

[ah, sh] = amphaseplot('invpend_bal', ...
  [-2*pi, 2*pi, 10], [-10, 10, 20], 0.15, ...
  [ ...
   -2*pi  2.14; -2*pi  3.4; -2*pi, 4.165; -2*pi  4.7; ...
    2*pi -2.14;  2*pi -3.4;  2*pi -4.165;  2*pi -4.7; ...
   -5.94  5; -2.9  5; -2.108  5;  0.1  5;  3  5;  3.7  5; ...
    5.94 -5;  2.9 -5;  2.108 -5; -0.1 -5; -3 -5; -3.7 -5 ...
  ], 30, a);
set(ah, 'DataAspectRatio', [1 1 1]);
amaxis([-2*pi 2*pi -4 4]); box on;
set(gca, 'XTick', [-2*pi -pi 0 pi 2*pi]);
set(gca, 'XTickLabel', {'-2pi', '-pi', '0pi' 'pi', '2pi'});

% Now add in the Lyapunov level set
A = [0, 1; 1 - 2*a, -1];
P = lyap(A', eye(2,2));

rho = 1.2;
theta = 0:0.1:pi;
r = 1 ./ sqrt((P(1,1)*(cos(theta).^2) + 2*P(1,2)*sin(theta).*cos(theta) + ...
               P(2,2)*(sin(theta).^2)));
xval = rho * r .* cos(theta);
yval = rho * r .* sin(theta);
plot(xval, yval, 'r-', -xval, -yval, 'r-', 'LineWidth', AM_thick_linewidth);

% Patch up the axis boundaries (screwed up by patch)
plot([-2*pi 2*pi  2*pi -2*pi], [-4 -4 4 4], 'k', 'LineWidth', 1);

amprint('invpend-stabilize.eps');
