% PendulumDTPID simulates pendulum under PID control.
% PID control implemented as difference equations
% with values held as pendulum is solved by ode23.
%
% function [tout,xout] = pendulumDTPID()
%
function [tout,xout] = pendulumDTPID()
global m l g B u

kp = 10; kd = 0; ki = 0; wc = 100; % DT PID Controller gains
m = 0.25; % kg      % pendulum parameters
l = 0.50; % m
g = 9.81; % m/s
B = 0.25; % Nms/rad
u = 0.00; % input torque

r      = 0.5; % reference value
T      = 0.01; % sample time
tstart = 0;
tfinal = 10;
x0     = [1, 0]; % initial conditions [thetao, dthetao/dt]
fi     = 0; % initial conditions for controller terms
fd     = 0;
eprev  = 0;

tout = tstart; % put initial values in output/storage variables
xout = x0;

% loop over time changing control every T seconds
while tstart < tfinal,
  % let system run first for a bit of time by solving DE
  [t,x] = ode23(@fpendulum, [tstart, tstart+T], x0);

  % accumulate output for plotting
  nt   = length(t);
  tout = [tout; t(2:nt)];
  xout = [xout; x(2:nt,:)];

  % implementation of discrete PID controller
  e  = r - x(nt,1);
  fi = fi + T*e;
  fd = (fd + wc*(e - eprev))/(1 + T*wc);
  u  = kp*e + ki*fi + kd*fd;
  eprev = e;

  % save last values for intitial conditions
  tstart = t(nt);
  x0     = x(nt,:);
end

plot(tout,xout(:,1),tout,xout(:,2))
xlabel('time (sec)'); ylabel('\theta (rad), d\theta/dt (rad/sec)');
legend('\theta','d\theta/dt');

% --------------------------------------------------------------
% fpendulum.m
% ODE/Vector Field Calculation for Pendulum Model
% x(1) = theta
% x(2) = dtheta/dt
function f = fpendulum(t,x)
global m l g B u

f = zeros(2,1); % initialize as column vector
f(1) = x(2);
f(2) = (u - m*g*l*sin(x(1)) - B*x(2))/(m*l^2);
% --------------------------------------------------------------