%%% Loop shaping for the DVD focus process
clear all;close all

% Define the process
s = zpk('s');
P = 6092*(63168-s)/(s^2+1553*s+718214);

% Plot open-loop Bode diagram
bode(P)
plotspecs
pause

% Try P controller
omega_c = 2000 * 2*pi;         % Find open-loop gain at desired 
g = abs(evalfr(P,i*omega_c));  % cross-over frequency 12566 rad/s
K = 1/g;
margin(K*P)
plotspecs
pause

% Add lag filter to increase LF gain
M = 15;     
a = 0.15*omega_c;
C_lag = (s+a)/(s+a/M);
C = K*C_lag;
margin(C*P)
plotspecs
pause

% Add lead filter to increase phase margin
N = 12;                  % Gives 58 degrees phase advance
b = omega_c/sqrt(N);     % Place maximum phase lead at cross-over freq
C_lead = N*(s+b)/(s+b*N);
g = abs(evalfr(C_lead*P,i*omega_c));
K = 1/g;
C = K*C_lag*C_lead;
margin(C*P)
plotspecs
pause

% LF-gain too low again, add another lag filter
C = K*C_lag^2*C_lead;
margin(C*P)
plotspecs
pause

% Phase margin a bit too low, decrease break frequency of lag filter
a = 0.08*omega_c;
C_lag = (s+a)/(s+a/M);
N = 12;    
b = omega_c/sqrt(N); % Place filter at cross-over freq
C_lead = N*(s+b)/(s+b*N);
g = abs(evalfr(C_lead*C_lag^2*P,i*omega_c));
K = 1/g;    % adjust gain to achieve cross-over freq wc
C = K*C_lag^2*C_lead;
margin(C*P)
plotspecs
pause

% Plot final controller Bode diagram
bode(C)
pause

% Plot Gang of Four
subplot(221)
bodemag(feedback(P*C,1))
subplot(222)
bodemag(feedback(P,C))
subplot(223)
bodemag(feedback(C,P))
subplot(224)
bodemag(feedback(1,C*P))

% Compute Ms and Mt
Ms = norm(feedback(1,P*C),inf)
Mt = norm(feedback(P*C,1),inf)
pause

% Plot closed-loop response to impulse disturbance
impulse(feedback(P,C))







