% PendulumCTPID simulates pendulum under PID control.
% PID control implemented as part of system
% dynamics which are solved by ode23.
%
% function [tout,xout] = pendulumCTPID()
%
function [tout,xout] = pendulumCTPID()
global m l g B u r rdot kp kd ki;

kp = 10; 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; rdot = 0.0; % reference value and its derivative
T      = 0.01; % sample time
tstart = 0;
tfinal = 10;
x0     = [1, 0, 0]; % initial condition [thetao, dthetao/dt, integral]

[t,x] = ode23(@fpendulum, [tstart, tfinal], x0);

plot(t,x(:,1),t,x(:,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
% x(3) = int(e) = int(r - theta)
function f = fpendulum(t,x)
global m l g B u r rdot kp kd ki

u = kp*(r - x(1)) + ki*x(3) + kd*(rdot - x(2));
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);
% --------------------------------------------------------------