%%
%% Cart Pendulum system
%%
%% This is the cart pendulum system that we have used in lectures and 
%% homework.  Some of the parameters have been changed and I have derived
%% the equations a bit differently, but the dynamics are basically the
%% same.
%%
%% Note: this is a linear model.  We should really do something nonlinear.
%%

aminit;					% initialize MATALB

M = 10;					% mass of cart, kg
m = 80;					% mass of pendulum, kg
b = 0.1;				% damping of cart, N/m/sec
l = 1;					% length of pendulum, m
J = 100;				% inertia of pendulum, kg m^2
g = 9.8;				% gravitational constant, kg m/sec^2

% Compute the inertia matrix for the system
InertiaMatrix = [M + m, -m*l; -m*l, J+m*l^2];

% Build the state space representation
Acp = [[zeros(2,2), eye(2,2)]; ...
     [InertiaMatrix \ [0, 0; 0, m*g*l], ...
      InertiaMatrix \ [-b, 0; 0, 0] ]];
Bcp = [[0; 0]; InertiaMatrix \ [1; 0]];
Ccp =[1 0 0 0];
Dcp = 0;

% Design a controler using LQR
rho = 0.01;
Kcp = lqr(Acp, Bcp, diag([1 1 1 1]), rho);

% Simulate the initial condition response
x0 = [1, 0, 0, 0];
Brf = [1; 0; 0; 0];			% reference input vector
t0 = 5;					% location of step
Tmax = 25;				% maximum time
cpsys=ss(Acp-Bcp*Kcp,Bcp*Kcp*Brf,Ccp,Dcp);
[y, t, x] = initial(cpsys, x0, Tmax-t0);

% Plot the response in the form needed for the book
figure(1); clf;				% clear the graphics window
subplot(221);				% set size of window
ploth = plot(t, x(:,1), t, x(:,2)*180/pi, 'b-');
set(ploth, 'LineWidth', AM_data_linewidth); 

% Label the lines
% lgh = legend('p', 'th'); legend(lgh, 'boxoff');
text(2.75, 0.35, 'th');
text(2.75, 1.05, 'p');

% Add reference lines
hold on; xy = plot([0, max(t)], [0, 0], 'k-');
set(xy, 'LineWidth', AM_ref_linewidth);

xlabel('time');
ylabel('position, angle');

amprint('cpinitial.eps');

