% pendulum.m
% Pendulum example showing use of ode solver, time plots,
% and phase portraits

clear all; close all; % clear memory and close all figures
global m l g B T
m = 0.25; %kg      % pendulum parameters
l = 0.50; %m
g = 9.81; %m/s
B = 0.25; %Nms/rad
T = 0.00; %input torque
tspan = [0 5];    % time span for solver, i.e., 0s<=t<=10s
figure(1); % open figure 1

% First Run (nearly vertical)
x0    = [-0.01 0]; % initial condition
% call ode45 to solve pendulum ODE
[t,x] = ode45(@fpendulum, tspan, x0);
% time plot
subplot(3,2,1); 
plot(t,x(:,1),'b-'); hold on;
plot(t,x(:,2),'r--'); hold off;
xlabel('time (s)'); ylabel('x_1, x_2');
legend('x_1', 'x_2'); title('Run 1: Time Plot of States');
% phase portrait plot w/vector field
minx1 = min(x(:,1)); maxx1 = max(x(:,1));
minx2 = min(x(:,2)); maxx2 = max(x(:,2));
x1step = (maxx1 - minx1)/10;
x2step = (maxx2 - minx2)/10;
[x1,x2] = meshgrid(minx1:x1step:maxx1, minx2:x2step:maxx2);
dx1 = x2;
dx2 = (T + m*g*l*sin(x1) - B*x2)/(m*l^2);
subplot(3,2,2);
axis([minx1, maxx1, minx2, maxx2]);
plot(x(:,1),x(:,2)); hold on;
quiver(x1, x2, dx1, dx2); hold off;
xlabel('x_1'); ylabel('x_2'); title('Run 1: Phase Portrait Plot');

% Second Run (no damping)
x0    = [1 0]; % initial condition
B = 0.0; %Nms/rad
% call ode45 to solve pendulum ODE
[t,x] = ode45(@fpendulum, tspan, x0);
% time plot
subplot(3,2,3); 
plot(t,x(:,1),'b-'); hold on;
plot(t,x(:,2),'r--'); hold off;
xlabel('time (s)'); ylabel('x_1, x_2');
legend('x_1', 'x_2'); title('Run 2: Time Plot of States');
% phase portrait plot w/vector field
minx1 = min(x(:,1)); maxx1 = max(x(:,1));
minx2 = min(x(:,2)); maxx2 = max(x(:,2));
x1step = (maxx1 - minx1)/10;
x2step = (maxx2 - minx2)/10;
[x1,x2] = meshgrid(minx1:x1step:maxx1, minx2:x2step:maxx2);
dx1 = x2;
dx2 = (T + m*g*l*sin(x1) - B*x2)/(m*l^2);
subplot(3,2,4);
axis([minx1, maxx1, minx2, maxx2]);
plot(x(:,1),x(:,2)); hold on;
quiver(x1, x2, dx1, dx2); hold off;
xlabel('x_1'); ylabel('x_2'); title('Run 2: Phase Portrait Plot');

% Third Run (constant input torque)
x0    = [-0.01 0]; % initial condition
B = 0.25; %Nms/rad
T = 1; %Nm
% call ode45 to solve pendulum ODE
[t,x] = ode45(@fpendulum, tspan, x0);
% time plot
subplot(3,2,5); 
plot(t,x(:,1),'b-'); hold on;
plot(t,x(:,2),'r--'); hold off;
xlabel('time (s)'); ylabel('x_1, x_2');
legend('x_1', 'x_2'); title('Run 3: Time Plot of States');
% phase portrait plot w/vector field
minx1 = min(x(:,1)); maxx1 = max(x(:,1));
minx2 = min(x(:,2)); maxx2 = max(x(:,2));
x1step = (maxx1 - minx1)/10;
x2step = (maxx2 - minx2)/10;
[x1,x2] = meshgrid(minx1:x1step:maxx1, minx2:x2step:maxx2);
dx1 = x2;
dx2 = (T + m*g*l*sin(x1) - B*x2)/(m*l^2);
subplot(3,2,6);
axis([minx1, maxx1, minx2, maxx2]);
plot(x(:,1),x(:,2)); hold on;
quiver(x1, x2, dx1, dx2); hold off;
xlabel('x_1'); ylabel('x_2'); title('Run 3: Phase Portrait Plot');

