% MAE 143 B
%
% Mauricio de Oliveira
%

format compact

% linear control design (P only)

g = 9.8
mp = 0.5
l = 0.3
r = l/2
J = mp*l^2/12

% model linearized around stable equilibiurm
b = 0
G = tf(1/(J+mp*r^2), [1, b/(J+mp*r^2), mp*r*g/(J+mp*r^2)])

den = G.den{1};
wn = sqrt(den(3)/den(1))
xi = den(2)/(2*wn*den(1))

% double the natural frequency
Kp = 3*mp*r*g

ctrP = Kp

H = feedback(ctrP*G,1)

den = H.den{1};
wn = sqrt(den(3)/den(1))
xi = den(2)/(2*wn*den(1))

wd = wn*sqrt(1 - xi^2)
fd = wd/(2*pi)
Td = 1/fd

figure(1), clf
subplot(1,2,1)
rlocus(ctrP*G), hold on
plot(pole(H), 'k*'), hold off
xlabel('Re')
ylabel('Im')
title('Root Locus (P controller)')

% Simulate nonlinear pendulum with feedback around stable equilibrium point

T = 2
thetaBar = 0
theta0 = pi/2

num = ctrP;
den = 1;

bs = [0 .1 .5];

N = length(bs);
y = cell(1,N);
t = cell(1,N);
i = 1;
for b = bs

  [ti,x,yi] = sim('SimplePendulumWithFeedback', T, simset('OutputVariables', 'ty'));
  t{i} = ti;
  y{i} = yi;

  i = i + 1;

end

leg = [ones(N, 1) * 'b = ' num2str(bs')];

figure(1)
subplot(1,2,2)
Y = [t; y];
plot(Y{:}, [0 T], thetaBar*[1 1], 'k--')
legend(leg, 'Location', 'NorthEast')
title(['$\bar{\theta} = ' num2str(thetaBar) '$'], 'FontSize', 15, 'Interpreter', 'latex')
xlabel('$t$', 'FontSize', 15, 'Interpreter', 'latex')
ylabel('$\theta(t)$', 'FontSize', 15, 'Interpreter', 'latex')
grid

% model linearized around unstable equilibiurm
b = 0
G = tf(1/(J+mp*r^2), [1, b/(J+mp*r^2), -mp*r*g/(J+mp*r^2)])

% use same K as before
H = feedback(ctrP*G,1)

den = H.den{1};
wn = sqrt(den(3)/den(1))
xi = den(2)/(2*wn*den(1))

wd = wn*sqrt(1 - xi^2)
fd = wd/(2*pi)
Td = 1/fd

figure(2), clf
subplot(1,2,1)
rlocus(ctrP*G), hold on
plot(pole(H), 'k*'), hold off
ylim(1.2*sort(imag(pole(H))))
xlabel('Re')
ylabel('Im')
title('Root Locus (P controller)')

% Simulate nonlinear pendulum with feedback around stable equilibrium point

T = 2
thetaBar = pi
theta0 = pi - pi/2

num = ctrP;
den = 1;

N = length(bs);
y = cell(1,N);
t = cell(1,N);
i = 1;
for b = bs

  [ti,x,yi] = sim('SimplePendulumWithFeedback', T, simset('OutputVariables', 'ty'));
  t{i} = ti;
  y{i} = yi;

  i = i + 1;

end

leg = [ones(N, 1) * 'b = ' num2str(bs')];

figure(2)
subplot(1,2,2)
Y = [t; y];
plot(Y{:}, [0 T], thetaBar*[1 1], 'k--')
legend(leg, 'Location', 'NorthEast')
title(['$\bar{\theta} = ' num2str(thetaBar) '$'], 'FontSize', 15, 'Interpreter', 'latex')
xlabel('$t$', 'FontSize', 15, 'Interpreter', 'latex')
ylabel('$\theta(t)$', 'FontSize', 15, 'Interpreter', 'latex')
grid