function [tout,xout] = pendulumCTPID()
global m l g B u r kp kd ki;

kp = 100; kd = 0.0; ki = 0.0; % CT 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, 0]; % initial condition [thetao, dthetao/dt, integral]

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,:)];

  % update discrete PID controller
%   e = r - x(nt);
%   ...
%   torque = ...;

  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 r kp kd ki

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