%%% Loop shaping for the DVD focus process

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

% Find open-loop gain at desired cross-over frequency 12566 rad/s
wc = 12566;
g = abs(freqresp(P,i*wc));

% Try P controller
K = 1/g;
margin(K*P)
plotspecs
pause

% Add lag filter to increase LF gain
M = 5;      % Raise LF-gain by factor 5
a = 0.1*wc; % Rule of thumb to not affect phase margin too much
Clag = (s+a)/(s+a/M);
C = K*Clag;
margin(C*P)
plotspecs
pause

% Add lead filter to increase phase margin
N = 10;         % Gives approx 55 deg phase advance
b = wc/sqrt(N); % Place filter at cross-over freq
Kk = 1/sqrt(N); % Give filter gain 1 at cross-over freq
Clead = Kk*N*(s+b)/(s+b*N);
C = K*Clag*Clead;
margin(C*P)
plotspecs
pause

% LF-gain now too low again, increase M and move breakpoint
M = 50;
a = 0.25*wc;
Clag = (s+a)/(s+a/M);
C = K*Clag*Clead;
margin(C*P)
plotspecs
pause

% Phase margin a bit too low, increase N
N = 15;     % keep b as before (nonstandard)
Clead = Kk*N*(s+b)/(s+b*N);
g = abs(freqresp(Clead*Clag*P,i*wc));
K = 1/g;    % adjust gain to achieve cross-over freq wc
C = K*Clag*Clead;
margin(C*P)
plotspecs
pause

% Nyquist plot
nyquist(P*C)
grid
pause

% Plot closed-loop step response
step(feedback(P*C,1))
pause

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

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





