% DISKPID  Digital PID servo control of a hard-disk drive - for SU 392m.
%
%   This code is hacked from a Matlab Control Toolbox demo DISKDEMO  
%   The code designs a digital servo controller for a disk drive read/write head.
% Run DISKDEMO for more detailed explanation 

% Acknowledgment: The model and data for this demo are taken from Chapter 14
% of "Digital Control of Dynamic Systems," by Franklin, Powell, and Workman.

clear all
close all

%
% Get the model from the data file 
%
load diskdemo
Gr = tf(1e6,[1 12.5 0],'inputdelay',1e-5);
Gf1 = tf(w1*[a1 b1*w1],[1 2*z1*w1 w1^2]);
Gf2 = tf(w2*[a2 b2*w2],[1 2*z2*w2 w2^2]);
Gf3 = tf(w3*[a3 b3*w3],[1 2*z3*w3 w3^2]);
Gf4 = tf(w4*[a4 b4*w4],[1 2*z4*w4 w4^2]);
G = Gr * (ss(Gf1) + Gf2 + Gf3 + Gf4);

%        
% Discretize
%
Ts = 7e-5;
Gd = c2d(G,Ts);

%
% Design controllers 
%    
C = tf(1,[1 -1],Ts);
C = C * zpk([.963,.963],-0.706,1,Ts);
C1 = 50 * C; 

% 
% Notch filter to suppress the flexible dynamics resonance 
% 
w0 = 4e3*2*pi;
notch = tf([1 2*0.06*w0 w0^2],[1 2*w0 w0^2]);
notchd = c2d(notch,Ts,'matched');


C2 = C1 * notchd * 2;
cl_step1 = feedback(Gd,C1);
cl_step2 = feedback(Gd,C2);
figure(1)
step(cl_step1,'r--',cl_step2,'b')
title('2nd-order compensator C1 (red) vs. 4th-order compensator C2 (blue)')
                
%
% Monte Carlo analysis of the parameter variations
%
[z2m,w2m,z3m,w3m] = ndgrid([.5*z2,1.5*z2],[.9*w2,1.1*w2],[.5*z3,1.5*z3],[.8*w3,1.2*w3]);
for j=1:16,  
    Gf2m(:,:,j) = tf(w2m(j)*[a2 b2*w2m(j)] , [1 2*z2m(j)*w2m(j) w2m(j)^2]);
    Gf3m(:,:,j) = tf(w3m(j)*[a3 b3*w3m(j)] , [1 2*z3m(j)*w3m(j) w3m(j)^2]);
end
G = Gr * (ss(Gf1) + Gf2m + Gf3m + Gf4);
Gdm = c2d(G,Ts);%

%
figure(2)
step(feedback(Gdm,C2))
title('Step disturbance rejection - Monte Carlo analysis') 
    
%
% Use the following workspace data for the assignment:
%   Ts sampling time 
%   Gr - rigid-body model (no flex modes) 
%   Gd - nominal sampled-time model 
%   Gdm - array of 16 perturbed models for Monte Carlo analysis 
%   notchd - notch filter to filter out the flexible resonance 
%   C2 - 4-th order controller designed in the demo - use it as a baseline to compare with your design 
%