function [tout,xout] = pendulumDTPID()
global m l g B u

kp = 10; kd = 0.0; ki = 0.0; Tf = 0.01; % 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]
ui     = 0; % initial conditions for controller terms
ud     = 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,:)];

  up = kp*(r - x(nt,1));
  ui = ui + ki*T*(r - x(nt,1));
  ud = ud/(1 + T/Tf) - kd/(Tf + T)*(x(nt,1)-x(1,1));
  u  = up + ui + ud;

  tstart = t(nt);
  x0     = x(nt,:);
end

plot(tout,xout(:,1),tout,xout(:,2))

% --------------------------------------------------------------
% 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);
% --------------------------------------------------------------