clear;
s = tf('s');

%% Problem 1, part (a)

% Compute damping / and natural frequency
omegan = sqrt(1e-3)
zeta = 4 / (2*omegan)
% Based on these values, the system is very overdamped, and the value
% for the natural frequency is not meaningful.

D = 2 / (1e-3*s^2 + 4e-3*s + 1e-6);
dpole = pole(D)

%% Problem 2, part (a)
% There is no overshoot

% Final value
dFinal = 2/1e-6;

% There is no overshoot, so settling within 5 percent is
dThresh = 0.95 * dFinal;

% The response will be dominated by the (much) smaller pole, so we can
% treat it as just a single exponential
t = linspace(0, 4e4, 1000);
y = 2e6 * (1 - exp(dpole(2) * t));

% Plot it with MATLAB's result just to confirm
clf;
step(D, t)
hold on;
plot(t, y, '--r');

% This will reach the threshold voltage at
tThresh = log( (dFinal - dThresh) / 2e6 ) / dpole(2);

% And plot it just to sanity-check
plot([tThresh, tThresh], [0, 2e6], ':k');
plot([0, 4e4], 2e6*[0.95, 0.95], ':k');

%% Problem 1, part (b)

F = (s + 1) / (12*s^2 + 6*s + 192);
fpole = pole(F)

omegan = sqrt(192/12)
zeta = 0.5 / (2*omegan)

% System is lightly damped

%% Problem 2, part (b)
clf;
step(F);
hold on;

% Particular solution
yfinal = 1/(12*sqrt(255));
t = linspace(0, 25, 500);
y = yfinal * (1 + (exp(-t/4) .* (4*sin(sqrt(255)/4*t) - cos(sqrt(255)/4*t))));
plot(t, y, 'r');

% For overshoot, we find the point where the derivative of the function is
% zero and solve for t.
% Have to throw in a +pi because tan by default gives us the one before t=0
tovershoot = (atan(-4) + pi) / (sqrt(255)/4);
overshoot_value = yfinal * (1 + (exp(-tovershoot/4) .* (4*sin(sqrt(255)/4*tovershoot) - cos(sqrt(255)/4*tovershoot))));
overshoot_pct = overshoot_value / yfinal * 100

%%
% For the settling time, we can just examine the bound of the exponential,
% which is the exp(-t/4) part, multiplied by the maximum value of the
% sin/cos part (sqrt(4^2 + 1^2)).
yfinal = 1/(12*sqrt(255));
ytop = yfinal * (1 + sqrt(17)*exp(-t/4));
ybottom = yfinal * (1 - sqrt(17)*exp(-t/4));
plot(t, ytop, '--k');
plot(t, ybottom, '--k');
%%
tThresh = -4 * log(0.05 / sqrt(17))
plot([tThresh, tThresh], [-1, 1], '-b');
plot([0, 25], yfinal*0.95*[1, 1], '-b');
plot([0, 25], yfinal*1.05*[1, 1], '-b');

%% Problem 1, part (c)
% This is unstable, so damping ratio is meaningless
roots([1, 0.01, -1])

%% Problem 3, part (a)
% We just need to change the constant term of the denominator, which can be
% done with proportional control alone.
kp = (4 - 0.001) / 2000;

H = feedback(kp*D, 1);
pole(H)
step(H);

%% Problem 3, part (b)
% Let's plot the root locus to get the lay of the land
rlocus(F);

%%
% If we can move the poles to real axis, we should have no overshoot
% Probing the root locus says this is at a gain of 115
% See paper work for gain calculation
kp_roots = roots([1, -36, -9180]);
kp = kp_roots(1) % First one is 115.48

H = feedback(kp*F, 1);

% The poles are now real
pole(H)
step(H);
% But the step response still overshoots (by a lot)!
% This is because of the zero (s+1) in the numerator dominates (it's closer
% to the imaginary axis than the poles).

% We have several options at this point:
% 1) Pull out pidTuner or Simulink and solve it by trial and error
% 2) Crank up kP beyond the critical damping point
% 3) Try a derivative or integral term

% Let's try #2.  Based on the root locus plot, if we increase kP above the
% critical damping point, one pole begins moving toward the zero.  As the pole
% gets closer to the zero, the overshoot decreases.  At infinite gain, the pole
% will be exactly on the zero, and there will be no overshoot.
clf; hold on;
step(F); % Original function

maxovershoot = 1.05 * 1/192;
tfinal = 1;

for kp = [150, 1000, 5e3, 1e4]
    H = feedback(kp*F, 1);
    % Calculate the ratio necessary to scale to match the final value
    ratio = (kp+192)/kp * (1/192);
    step(ratio*H, tfinal);
end

plot([0, tfinal], maxovershoot*[1, 1], '--k')
ylim([0, 0.01]);

% To get a proper analytical solution, we could take the inverse Laplace
% transform of this and solve for kp, but that gets very nasty.

%%
% If we use an integral term, we're effectively introducing another pole into
% the transfer function.  If we place this to the right of the zero, we can
% offset its effect and reduce the overshoot.
clf; hold on;
tfinal = 5;
kp = 150;
for ki = [100, 500, 1000]
    C = kp + ki/s;
    H = feedback(C*F, 1);
    step(H, tfinal);
end

plot([0, tfinal], 1.05*[1, 1], '--k')

%% Problem 3, part (c)
% Again, let's start by examining the root locus
G = 1200 / (s^2 + 0.01*s - 1);
rlocus(G);

% If we used proportional control and found the critical damping, the system
% would respond far too slowly (roots are at -0.005)
% So we're gonna need a higher proportional gain and a derivative term

% Math on paper
% Want no overshoot, so poles should be real
% To get to 90% of the final value in < 1 second, want poles < log(0.1) = -2.3

% Let's put both poles at -5.
wn = 5; % zeta = 1, for no oscillation
kd = (2*wn-0.01)/1200;
kp = (wn^2 + 1)/1200;

C = kp + s*kd;

H = feedback(C*G, 1);

pole(H)
step(H, 10)
% Aaagh! The PD control also introduces a zero, which causes overshoot.
% The zero will be at -kp/kd:
z = -kp/kd
% There's no way to position a pole to the right of the zero while also having
% it be less than -2.3.  For very large gains, the pole is almost on top of the
% zero, which limits the overshoot.

%%
% So we're gonna need an integral term, to introduce another pole
wn = 5; % zeta = 1, for no oscillation
kd = (2*wn-0.01)/1200;
kp = (wn^2 + 1)/1200;
ki = 0.001;
C = kp + s*kd + ki/s;

rlocus(C*G);
H = feedback(C*G, 1);
% step(H);
