% balance_place.m - stabilization of balance system by pole placement
% RMM, 24 Mar 07

aminit					% MATLAB initialization

% Parameter definitions (roughly a human on a Segway)
balance_params;

% System dynamics
A = [
  0  0              1          0;
  0  0              0          1;
  0  m^2*l^2*g/det  -c*Jt/det  -gam*l*m/det;
  0  Mt*m*g*l/det   -c*l*m/det -gam*Mt/det
];
B = [0; 0; Jt/det; l*m/det];
C = [1 0 0 0];

% Compute the open loop eigenvalues and print them as a check for the text
eigs(A)

%
% First design - aggressive goals that will generate large forces
%

% Figure out where to place the eigenvalues
zeta0 = 0.5; omega0 = sqrt(m*g*l/(J+m*l^2));
zeta1 = 0.7; omega1 = 0.5;
lam3 = -zeta1*omega1 + omega1*sqrt(1-zeta1^2)*i;
lam4 = -zeta1*omega1 - omega1*sqrt(1-zeta1^2)*i;
evdes = [-1 + 2i, -1 - 2i, lam3, lam4]

% Design the controller and construct the closed loop dynamics
K1 = place(A, B, evdes)
kr = -1/(C * inv(A-B*K1) * B);

% Create a system that gives output and control action
sys = ss(A-B*K1, B*kr, [C; -K1], [0; kr]);

% Plot the reponse for the system
[Y, T] = step(sys, 16);

figure(1); clf; subplot(321);
plot(T, Y(:,1), 'b', 'LineWidth', AM_data_linewidth);
hold on; plot([0 16], [1 1], 'k', 'LineWidth', AM_ref_linewidth);
amaxis([0 16 -0.2 2]);
ylabel('Position (m)');

subplot(323);
plot(T, Y(:,2), 'b', 'LineWidth', AM_data_linewidth);
amaxis([0 16 -10 30]);
ylabel('Input force (N)');
xlabel('Time (sec)');

amprint('balance-step1.eps');

%
% Second design - scaled back goals
%

zeta0 = zeta0; omega0 = omega0/3;
zeta1 = zeta1; omega1 = omega1/2;
Tmax = 40;
lam3 = -zeta1*omega1 + omega1*sqrt(1-zeta1^2)*i;
lam4 = -zeta1*omega1 - omega1*sqrt(1-zeta1^2)*i;
evdes = [-zeta0*omega0 + omega0*i, -zeta0*omega0 - omega0*i, lam3, lam4]

% Design the controller and construct the closed loop dynamics
K2 = place(A, B, evdes)
kr = -1/(C * inv(A-B*K2) * B);

% Create a system that gives output and control action
sys = ss(A-B*K2, B*kr, [C; -K2], [0; kr]);

% Plot the reponse for the system
[Y, T] = step(sys, Tmax);

figure(2); clf; subplot(321);
plot(T, Y(:,1), 'b', 'LineWidth', AM_data_linewidth);
hold on; plot([0 Tmax], [1 1], 'k', 'LineWidth', AM_ref_linewidth);
amaxis([0 Tmax -0.2 2]);
ylabel('Position (m)');

subplot(323);
plot(T, Y(:,2), 'b', 'LineWidth', AM_data_linewidth);
amaxis([0 Tmax -10 30]);
ylabel('Input force (N)');
xlabel('Time (sec)');

amprint('balance-step2.eps');

%
% Third design - LQR
%

% Design a controler using LQR
rho = 0.01;
K3 = lqr(A, B, eye(length(B)), rho);
kr = -1/(C * inv(A-B*K3) * B);

% Create a system that gives output and control action
sys = ss(A-B*K3, B*kr, [C; -K3], [0; kr]);

% Plot the reponse for the system
[Y, T] = step(sys, Tmax);

figure(3); clf; subplot(321);
plot(T, Y(:,1), 'b', 'LineWidth', AM_data_linewidth);
hold on; plot([0 Tmax], [1 1], 'k', 'LineWidth', AM_ref_linewidth);
amaxis([0 Tmax -0.2 2]);
ylabel('Position (m)');

subplot(323);
plot(T, Y(:,2), 'b', 'LineWidth', AM_data_linewidth);
amaxis([0 Tmax -1 8]);
ylabel('Input force (N)');
xlabel('Time (sec)');

amprint('balance-step3.eps');
