function PendCtrl

% function PendCtrl
% Inverted pendulum control simulation.

close all; % close figures

dt = 0.1; % simulation step size
tf = 20; % length of simulation
ref = 2; % reference input for cart position

m = 0.2; % pendulum mass
M = 1.0; % cart mass
g = 9.81; % acceleration due to gravity
F = 0.1; % coefficient of viscous friction on cart wheels
l = 1; % length of pendulum
r = 0.02; % radius of pendulum mass
J = m * r * r / 2; % moment of inertia of pendulum mass (cylinder)

theta = -0.5; % Initial pendulum angle.
thetadot = 0;
thetadotdot = 0;

y = -1; % Initial cart position.
ydot = 0;
ydotdot = 0;

theta_array = [];
y_array = [];

% Linearized system matrices.
A = [0       1             0   0;
     0    -F/M        -m*g/M   0;
     0       0             0   1;
     0   F/M/l   (M+m)*g/M/l   0];
B = [0; 1/M; 0; -1/M/l];
C = [1 0 0 0];
D = [0];

% Find the feedback matrix to place the 
% desired closed loop eigenvalues.
p = [-1.5+0.5j -1.5-0.5j -1+j -1-j];
k = place(A, B, p);

% Find the feedforward gain to track a reference input.
[num, den] = ss2tf(A, B, C, D);
beta = num(size(num, 2));
dbar = poly(p);
alpha = dbar(size(dbar, 2));
p = alpha / beta;

for t = 0 : dt : tf
  % Save the old state values.
  theta_old = theta;
  thetadot_old = thetadot;
  thetadotdot_old = thetadotdot;
  y_old = y;
  ydot_old = ydot;
  ydotdot_old = ydotdot;
  sine = sin(theta);
  cosine = cos(theta);
  % Compute the feedback control.
  x = [y; ydot; theta; thetadot];
  u = p * ref - k * x;
  % Compute the new state values.
  theta = theta + thetadot_old * dt;
  thetadot = thetadot + thetadotdot_old * dt;
  thetadotdot = (m*g*l*sine - m*l*cosine*(u + ...
     m*l*thetadot_old*thetadot_old*sine - F*ydot_old) / (M + m)) / ...
     (J + m*l*l - m*m*l*l*cosine*cosine / (M + m));
  y = y + ydot_old * dt;
  ydot = ydot + ydotdot_old * dt;
  ydotdot = (u - m*l*thetadotdot_old*cosine + m*l*thetadot_old*thetadot_old*sine ...
    - F*ydot_old) / (M + m);
  % Save the state values in arrays for later plotting.
  theta_array = [theta_array theta];
  y_array = [y_array y];
end
% Plot results
t = [0 : dt : tf];
figure;
subplot(2,1,1);
plot(t, (180 / pi) * theta_array);
ylabel('angle (degrees)');
subplot(2,1,2);
plot(t, y_array);
ylabel('position (meters)');
xlabel('time (seconds)');