% balance_polezero.m - pole zero diagrams for balance system
% RMM, 8 Jul 07

aminit;				% initialize path, parameters
balance_params;			% load balance system parameters

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

sys1 = ss(A, B, C1, 0);
sys2 = ss(A, B, C2, 0);

figure(1); clf; subplot(321);
ampolezero(sys1, [-5 5 -2 2]);

% Add axis labels
hold on; plot([-4 -2 0 2 4], [0 0 0 0 0], 'k+', 'LineWidth', 0.4);
text([-4 -2 0 2 4] - 0.3, [0 0 0 0 0] - 0.5, {'-4', '-2', '', ' 2', ' 4'});

hold on; plot([0 0], [-1 1], 'k+', 'LineWidth', 0.4);
text([0 0] + 0.5, [-1 1], {'-1', '1'});

amprint('balance-polezero-1.eps');

figure(2); clf; subplot(323);
ampolezero(sys2, [-5 5 -2 2]);

% Add axis labels
hold on; plot([-4 -2 0 2 4], [0 0 0 0 0], 'k+', 'LineWidth', 0.4);
text([-4 -2 0 2 4] - 0.3, [0 0 0 0 0] - 0.5, {'-4', '-2', '', ' 2', ' 4'});

hold on; plot([0 0], [-1 1], 'k+', 'LineWidth', 0.4);
text([0 0] + 0.5, [-1 1], {'-1', '1'});

amprint('balance-polezero-2.eps');

% Print out the poles and zeros for the system with zero damping
% System dynamics
A0 = [
  0  0              1          0;
  0  0              0          1;
  0  m^2*l^2*g/det  0	       0;
  0  Mt*m*g*l/det   0	       0
];
sys0 = ss(A0, B, C2, 0);
zpk(sys0)
