% pole_balance.m

% Controller design for inverted pendulum presented in class.

format compact
clc    % clear command window
clear  % clear MatLab memory

% system parameters
M = 1;  m = 0.1;  l = 0.5;  g = 9.81;
A = 2/3*l*(M + m) - m*l/2
B = g*(M + m)

% Uncompensated open loop function
% Gp = 1 / (-A*s^2 + B)
N = [1];
D = [-A 0 B];
Gp = tf (N, D)
GpH = Gp;   % H = 1

% Root Locus
rlocus (Gp)

% PID controllers with various integral gains and 50 degree phase margin
%   (analysis from mypid.m)
Ki = input ('Enter desired Ki value: ');
Ts = input ('Enter desired settling time (Ts): ');
pm = 50*pi/180; disp('pm = 50')
w1 = 8 / (Ts * tan (pm));
GpHjw1 = evalfr(GpH, j*w1);
mag = abs(GpHjw1);
theta = -pi + pm - angle (GpHjw1);
Kp = cos(theta) / mag
figure('Name', 'PID IC response');
Kd = ((sin(theta) / mag) + Ki/w1) / w1
Gcpid = tf([Kd Kp Ki], [1 0]);
T = minreal((Gcpid*Gp / (1 + Gcpid*GpH)));
ssT = ss(T);    % convert transfer function to state-space model
[A,B,C,D] = ssdata(ssT);
ICs = zeros (size(B));
ICs(1) = 0.1;    % 0.1 rad IC on theta
initial (ssT, ICs, 5)    % simulate response given IC's
figure ('Name', 'PID Bode'); bode (Gcpid*GpH)
figure ('Name', 'PID Nyquist'); nyquist (Gcpid*GpH)

% Verify stability by looking at closed loop poles
OLF = minreal (Gcpid*Gp);
open_loop_poles = pole (OLF)
T = minreal (Gcpid*Gp / (1 + Gcpid*GpH))
closed_loop_poles = pole (T)
time_constants = -1 ./ real(closed_loop_poles)

% pause for the user and remove figures
display ('Hit Enter to continue');
pause
close all