function slide = diskdemo(slidenum)
%DISKDEMO  Digital servo control of a hard-disk drive.
%
%   This demo shows how to use the Control System Toolbox to design 
%   a digital servo controller for a disk drive read/write head.

%   Author: P. Gahinet 8/2000
%   Copyright 1986-2000 The MathWorks, Inc. 
%   $Revision: 1.16 $  $Date: 2000/08/24 22:21:21 $

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

persistent Gr G Gd Ts C C1 C2 Gf1 Gf4

if nargin
    %---Slide code
    cla, legend off, hold off
    switch slidenum
    case 1
        % Disk drive picture
        set(gca,'Position',[0.085 0.49-0.05 0.65 0.45],'visible','on',...
            'XTick',[],'YTick',[],'box','on','Ylim',[.1 1.1],'Xlim',[0 1])
        axis square, hold on
        dskwheel(.5,.6,.3);
        dskwheel(0.2109,0.2670,0.05);
        x=[0.2487,0.5563,0.5685,0.2701,0.2518]-0.05;
        y=[0.2853,0.3980,0.3736,0.2457,0.2853];
        fill(x,y,'w');
        plot(0.5,0.5,'k.','MarkerSize',20);
        title('Disk Platen - Read/Write Head');
 
    case 2
        % Model
        axis normal
        set(gca,'Position',[0.1+.015 0.49 0.64-.01 0.45],...
            'xlim',[0 1],'ylim',[0 1],'vis','off','ydir','normal')
        text(-.05,.9,'Disk Drive Model:','fontweight','bold','fontsize',14);
        text(-.02,.4,'G(s) = G_{r}(s) G_{f}(s)','Hor','left','fontsize',12+2*isunix,'fontweight','b')
        equation('position',[.42 .6],'name','G_r(s)','gain','e^{-1e-5 s}',...
            'num','1e6','den','s(s+12.5)',...
            'Anchor','left','fontsize',10+2*isunix,'color','r');
        equation('position',[.42 .2],'name','G_f(s)','Sigma',1,'SigmaBounds',{'i=1','4'},...
            'num','\omega_j (a_j s + b_j  \omega_j)',...
            'den','s^2 + 2 \zeta_{ j} \omega_j s + \omega_j^2',...
            'Anchor','left','fontsize',10+2*isunix,'color','r');
        
    case 3
        % Model data
        axis normal
        set(gca,'xlim',[0 1],'ylim',[0 1],'vis','off','ydir','normal')
        text(-.05,.9,'Model Data:','fontweight','bold','fontsize',14);
        text(.05,.6,'(a_1,b_1,\zeta_1,\omega_1) = (.0000115,-.00575,.05,70)','fontsize',10+2*isunix,'color','b');
        text(.05,.4,'(a_2,b_2,\zeta_2,\omega_2) = (0,.0230,.005,2200)','fontsize',10+2*isunix,'color','b');
        text(.05,.2,'(a_3,b_3,\zeta_3,\omega_3) = (0,.8185,.05,4000)','fontsize',10+2*isunix,'color','b');
        text(.05,.0,'(a_4,b_4,\zeta_4,\omega_4) = (.0273,.1642,.005,9000)','fontsize',10+2*isunix,'color','b');
        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);
        
    case 4
        axis normal
        set(G,'inputname','ic','outputname','PES')
        bode(G)
        set(gcr,'FrequencyUnits','Hz','MagnitudeUnits','dB','PhaseUnits','deg',...
            'Grid','off','Xlim',[1 1e5],'Title','Bode diagram of the head assembly model')
        
    case 5
        LocalDrawLoop
        
    case 6
        axis normal
        set(gca,'xlim',[0 1],'ylim',[0 1],'vis','off','ydir','normal')
        text(-.05,.9,'Design Specs:','fontweight','bold','fontsize',14);
        text(.05,.65,'Open-loop gain > 20dB at 100 Hz','fontsize',10+2*isunix,'color','r');
        text(.05,.5,'Bandwidth > 800 Hz','fontsize',10+2*isunix,'color','r');
        text(.05,.35,'Gain margin > 10 dB','fontsize',10+2*isunix,'color','r');
        text(.05,.2,'Phase margin > 45 deg','fontsize',10+2*isunix,'color','r');
        text(.05,.05,'Peak closed-loop gain < 4 dB','fontsize',10+2*isunix,'color','r');
        
    case 7
        % Discretize
        axis normal
        Ts = 7e-5;
        Gd = c2d(G,Ts);
        bode(G,'b',Gd,'r')
        set(gcr,'FrequencyUnits','Hz','MagnitudeUnits','dB','PhaseUnits','deg',...
            'Grid','off','Xlim',[1 1e5],'Title','Continuous (blue) and discretized (red) HDA models')
        
    case 8
        % Root locus plot
        C = tf(1,[1 -1],Ts);
        rlocus(Gd*C)
        set(gcr,'Xlim',[-1.5 1.5],'ylim',[-1 1],'grid','on')
        
    case 9
        C = C * zpk([.963,.963],-0.706,1,Ts);
        rlocus(Gd*C)
        set(gcr,'Xlim',[-1.25 1.25],'ylim',[-1.2 1.2],'grid','on')
        C1 = 50 * C;
        
    case 10
        % Closed-loop response
        cl_step = feedback(Gd,C1);
        step(cl_step)
        set(gcr,'Grid','off','Title','Rejection of a step disturbance (PES = position error)')
        
    case 11
        margin(Gd*C1)
        set(gcr,'FrequencyUnits','Hz','MagnitudeUnits','dB','PhaseUnits','deg',...
            'Xlim',[1e2 1e4],'grid','on','Ylim',{[-40 20],[-450 360]})
        
    case 12
        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;
        bode(notchd)
        set(gcr,'FrequencyUnits','Hz','MagnitudeUnits','dB','PhaseUnits','deg',...
            'grid','on','title','Discrete-time notch filter')        
        
    case 13
        margin(Gd*C2)
        set(gcr,'FrequencyUnits','Hz','MagnitudeUnits','dB','PhaseUnits','deg',...
            'Xlim',[1e2 1e4],'grid','on','Ylim',{[-40 30],[-450 360]})        
        
    case 14
        cl_step1 = feedback(Gd,C1);
        cl_step2 = feedback(Gd,C2);
        step(cl_step1,'r--',cl_step2,'b')
        set(gcr,'Grid','off','Title','2nd-order compensator C1 (red) vs. 4th-order compensator C2 (blue)')
        
    case 15
        T = feedback(Gd*C2,1);
        bodemag(T)
        set(gcr,'FrequencyUnits','Hz','MagnitudeUnits','dB',...
            'Xlim',[1e2 1e4],'grid','on','PeakResponse','on',...
            'Title','Peak response of closed-loop sensitivity T(s)') 
        
    case 16
        set(gca,'xlim',[0 1],'ylim',[0 1],'vis','off','ydir','normal')
        text(-.05,.9,'Parameter Variations:','fontweight','bold','fontsize',14);
        text(.05,.6,'\omega_2 = 2200 \pm 10%','fontsize',10+2*isunix,'color','r');
        text(.05,.4,'\omega_3 = 4000 \pm 20%','fontsize',10+2*isunix,'color','r');
        text(.05,.2,'\zeta_2 = 0.005 \pm 50%','fontsize',10+2*isunix,'color','r');
        text(.05,.0,'\zeta_3 = 0.05 \pm 50%','fontsize',10+2*isunix,'color','r');
        
    case 17
        % Parameter variations
        load diskdemo
        [z2,w2,z3,w3] = 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,  
           Gf2(:,:,j) = tf(w2(j)*[a2 b2*w2(j)] , [1 2*z2(j)*w2(j) w2(j)^2]);
           Gf3(:,:,j) = tf(w3(j)*[a3 b3*w3(j)] , [1 2*z3(j)*w3(j) w3(j)^2]);
        end
        G = Gr * (ss(Gf1) + Gf2 + Gf3 + Gf4);
        Gd = c2d(G,Ts);
        bode(Gd*C2)
        set(gcr,'FrequencyUnits','Hz','MagnitudeUnits','dB',...
            'Xlim',[8e2 8e3],'grid','on',...
            'Title','Open-loop response - Monte Carlo analysis') 
        
    case 18
        axis normal
        step(feedback(Gd,C2))
        set(gcr,'Grid','off','Title','Step disturbance rejection - Monte Carlo analysis') 
    
    end
    
    
elseif nargout<1
    %---Start demo
    fg = playshow(mfilename);
    set(fg,'Name','Disk Drive Demo','doublebuffer','on');
    
    
else
    %---Construct slides
    slidenum = 0;
    
    %========== Slide 1 ==========
    slidenum = slidenum+1;
    slide(slidenum).code={sprintf('%s(%d)',mfilename,slidenum)};
    slide(slidenum).text={
        ' Press the "Start" button to see how you can use the Control System Toolbox'
        ' to design a digital servo controller for a disk drive read/write head.'
        ' '
        ' For details about the system and model, see Chapter 14 of "Digital Control'
        ' of Dynamic Systems," by Franklin, Powell, and Workman.'
      };    
    
    %========== Slide 2 ==========
    slidenum = slidenum+1;
    slide(slidenum).code={sprintf('%s(%d)',mfilename,slidenum)};
    slide(slidenum).text={
        ' The head-disk assembly (HDA) and actuators are modeled by a'
        ' 10th-order transfer function including two rigid-body modes and'
        ' the first four resonances. '
        ' '
        ' The model input is the current ic driving the voice coil motor, and'
        ' the output is the position error signal (PES, in % of track width).'
        ' The model also includes a small delay.'
    };
    
    %========== Slide 3 ==========
    slidenum = slidenum+1;
    slide(slidenum).code={sprintf('%s(%d)',mfilename,slidenum)};
    slide(slidenum).text={
        ' The coupling coefficients, damping, and natural frequencies (in Hz) for'
        ' the dominant flexible modes are listed above.  Given this data, construct'
        ' a nominal model of the head assembly:'
        ' '
        ' >> Gr = tf(1e6 , [1 12.5 0] , ''inputdelay'',1e-5)'
        ' >> Gf1 = tf(w1*[a1 b1*w1] , [1 2*z1*w1 w1^2])       % first resonance'
        ' >>   ...'
        ' >> Gf4 = tf(w4*[a4 b4*w4] , [1 2*z4*w4 w4^2])       % fourth resonance'
        ' >> G = Gr * (ss(Gf1) + Gf2 + Gf3 + Gf4)  '
        '                                                    % convert to state space for accuracy'
    };
    
    %========== Slide 4 ==========
    slidenum = slidenum+1;
    slide(slidenum).code={sprintf('%s(%d)',mfilename,slidenum)};
    slide(slidenum).text={
        ' Plot the Bode response of the head assembly model:'
        ' '
        ' >> set(G, ''inputname'',''ic'', ''outputname'',''PES'')           % label I/Os'
        ' >> bode(G)'
    };
    
    %========== Slide 5 ==========
    slidenum = slidenum+1;
    slide(slidenum).code={sprintf('%s(%d)',mfilename,slidenum)};
    slide(slidenum).text={
        ' Servo control is used to keep the read/write head "on track."   The servo'
        ' controller C(z) is digital and designed to maintain the PES (offset from'
        ' the track center) close to zero.'
        ' '
        ' The disturbance considered here is a step variation d in the input current ic.'
        ' Your task is to design a digital compensator C(z) with adequate disturbance'
        ' rejection performance.'
    };
    
    %========== Slide 6 ==========
    slidenum = slidenum+1;
    slide(slidenum).code={sprintf('%s(%d)',mfilename,slidenum)};
    slide(slidenum).text={
        ' The sampling time for the digital servo is Ts = 7e-5 sec (14.2 kHz).'
        ' '
        ' Realistic design specs are listed above.' 
    };
    
    %========== Slide 7 ==========
    slidenum = slidenum+1;
    slide(slidenum).code={sprintf('%s(%d)',mfilename,slidenum)};
    slide(slidenum).text={
        ' Since the servo controller is digital, you can perform the design in the '
        ' discrete domain.  To this effect, discretize the HDA model using C2D'
        ' and the zero-order hold (ZOH) method:'
        ' '
        ' >> Ts = 7e-5'
        ' >> Gd = c2d(G,Ts)'
        ' >> bode(G,''b'',Gd,''r'')        % compare with the continuous-time model'
    };
    
    %========== Slide 8 ==========
    slidenum = slidenum+1;
    slide(slidenum).code={sprintf('%s(%d)',mfilename,slidenum)};
    slide(slidenum).text={
        ' Now to the compensator design.  Start with a pure integrator 1/(z-1)'
        ' to ensure zero steady-state error:'
        ' '
        ' >> C = tf(1,[1 -1],Ts)'
        ' '
        ' Plot the root locus of the open-loop model Gd*C'
        ' '
        ' >> rlocus(Gd*C)'
        ' ' 
        ' and zoom around z=1 using the right-click menu.'
    };
    
    %========== Slide 9 ==========
    slidenum = slidenum+1;
    slide(slidenum).code={sprintf('%s(%d)',mfilename,slidenum)};
    slide(slidenum).text={
        ' Because of the two poles at z=1, the servo loop is unstable for all positive'
        ' gains.  To stabilize the feedback loop, first add a pair of zeros near z=1:'
        ' '
        ' >> C = C * zpk([.963,.963],-0.706,1,Ts)'
        ' >> rlocus(Gd*C)'
        ' '
        ' Next adjust the loop gain by clicking on the locus and dragging the black'
        ' square inside the unit circle.  The loop gain is displayed in the data marker.'
        ' A gain of approximately 50 stabilizes the loop (set C1 = 50*C).'
    };
    
    %========== Slide 10 ==========
    slidenum = slidenum+1;
    slide(slidenum).code={sprintf('%s(%d)',mfilename,slidenum)};
    slide(slidenum).text={
        ' Now simulate the closed-loop response to a step disturbance in current:'
        ' '
        ' >> cl_step1 = feedback(Gd,C1)'
        ' >> step(cl_step1)'
        ' '
        ' The disturbance is smoothly rejected, but the PES is too large (head'
        ' deviates from track center by 45% of track width).'
    };
    
    %========== Slide 11 ==========
    slidenum = slidenum+1;
    slide(slidenum).code={sprintf('%s(%d)',mfilename,slidenum)};
    slide(slidenum).text={
        ' Next look at the open-loop Bode response and the stability margins:'
        ' '
        ' >> margin(Gd * C1)'
        ' '
        ' The gain at 100 Hz is only 15 dB (vs. spec of 20 dB) and the gain margin'
        ' is only 7dB, so increasing the loop gain is not an option.'
        
    };
        
    %========== Slide 12 ==========
    slidenum = slidenum+1;
    slide(slidenum).code={sprintf('%s(%d)',mfilename,slidenum)};
    slide(slidenum).text={
        ' To make room for higher low-frequency gain, add a notch filter near the '
        ' 4000 Hz resonance:'
        ' '
        ' >> w0 = 4e3 * 2*pi                                                % notch frequency in rad/sec'
        ' >> notch = tf([1 2*0.06*w0 w0^2],[1 2*w0 w0^2])    % continuous-time notch'
        ' >> notchd = c2d(notch,Ts,''matched'')                        % discrete-time notch'
        ' >> C2 = C1 * notchd'
        ' '
        ' Note: The SISO Design Tool is ideal for interactively tuning these compensator'
        '           parameters.  See the web version of this demo for a live showcase.'
    };
        
    %========== Slide 13 ==========
    slidenum = slidenum+1;
    slide(slidenum).code={sprintf('%s(%d)',mfilename,slidenum)};
    slide(slidenum).text={
        ' You can now safely double the loop gain:'
        ' '
        ' >> C2 = 2 * C2'
        ' >> margin(Gd * C2)'
        ' '
        ' The resulting stability margins and gain at 100 Hz are within specs.'
    };
    
    %========== Slide 14 ==========
    slidenum = slidenum+1;
    slide(slidenum).code={sprintf('%s(%d)',mfilename,slidenum)};
    slide(slidenum).text={
        ' Step disturbance rejection has also greatly improved:'
        ' '
        ' >> cl_step2 = feedback(Gd,C2)'
        ' >> step(cl_step1,''r--'',cl_step2,''b'')'
        ' '
        ' The PES now stays below 20% of the track width.'
    };
    
    
    %========== Slide 15 ==========
    slidenum = slidenum+1;
    slide(slidenum).code={sprintf('%s(%d)',mfilename,slidenum)};
    slide(slidenum).text={
        ' Check if the 3dB peak gain spec on   T = Gd*C/(1+Gd*C)   (closed-loop '
        ' sensitivity) is met:'
        ' '
        ' >> bodemag(feedback(Gd*C,1))'
        ' '
        ' To read the peak value, hold the mouse over the blue marker, or just'
        ' click on it.'
    };
    
    %========== Slide 16 ==========
    slidenum = slidenum+1;
    slide(slidenum).code={sprintf('%s(%d)',mfilename,slidenum)};
    slide(slidenum).text={
        ' Finally let''s analyze the robustness to variations in the damping and natural'
        ' frequencies of the 2nd and 3rd flexible modes.  Generate an array of 16'
        ' models corresponding to all combinations of extremal values of z2,w2,z3,w3:'
        ' '
        ' >> [z2,w2,z3,w3] = ndgrid([.5,1.5]*z2,[.8,1.2]*w2,[.5,1.5]*z3,[.8,1.2]*w3)'
        ' >> for j=1:16, '
        '          Gf2(:,:,j) = tf(w2(j)*[a2  b2*w2(j)] , [1  2*z2(j)*w2(j)  w2(j)^2])'
        '          Gf3(:,:,j) = tf(w3(j)*[a3  b3*w3(j)] , [1  2*z3(j)*w3(j)  w3(j)^2])'
        '     end'
        ' >> G = Gr * (ss(Gf1) + Gf2 + Gf3 + Gf4)           % LTI array with 16 models'
    };
        
    %========== Slide 17 ==========
    slidenum = slidenum+1;
    slide(slidenum).code={sprintf('%s(%d)',mfilename,slidenum)};
    slide(slidenum).text={
        ' Discretize these 16 models at once and see how the parameter variations'
        ' affect the open-loop response:'
        ' '
        ' >> Gd = c2d(G,Ts)'
        ' >> bode(Gd * C2)'
        ' '
        ' Note: You can click on any curve to identify the underlying model.'
    };
    
    %========== Slide 18 ==========
    slidenum = slidenum+1;
    slide(slidenum).code={sprintf('%s(%d)',mfilename,slidenum)};
    slide(slidenum).text={
        ' Plot the step disturbance rejection performance for these 16 models:'
        ' '
        ' >> step(feedback(Gd,C2))'
        ' '
        ' All 16 responses are nearly identical: our servo design is robust!' 
     };
    
end 

%---------------------- Local Functions -----------------------------------


function LocalDrawLoop
% Draws control loop
axis equal
ax = gca;
set(ax,'visible','off','xlim',[-1 18],'ylim',[0 12])
y0 = 7;  x0 = 0;
if isunix, 
    fw = 'normal';
else
    fw = 'bold';
end
wire('x',x0+[0 2],'y',y0+[0 0],'parent',ax,'arrow',0.5);
text(x0,y0,'d ','horiz','right','fontweight',fw);
sumblock('position',[x0+2.5,y0],'label','-235','radius',.5,'fontsize',15);
wire('x',x0+[3 7],'y',y0+[0 0],'parent',ax,'arrow',0.5);
sysblock('position',[x0+7 y0-2 4 4],'name','G(s)',...
    'fontweight','bold','facecolor',[1 1 .9],'fontsize',12+2*isunix);
wire('x',x0+[11 16],'y',y0+[0 0],'parent',ax,'arrow',0.5);
text(x0+16,y0,' PES','horiz','left','fontweight',fw);
wire('x',x0+[14 14 13],'y',y0+[0 -6 -6],'parent',ax);
wire('x',x0+[13 13-.707],'y',y0+[-6 -6+.707],'parent',ax);
wire('x',x0+[12 10.5],'y',y0+[-6 -6],'parent',ax,'arrow',0.5);
sysblock('position',[x0+7.5 y0-7.5 3 3],'name','C(z)',...
    'fontweight','bold','facecolor',[.8 1 1],'fontsize',12+2*isunix);
text(x0+12.5,y0-7,'Ts','horiz','center','fontweight',fw);
wire('x',x0+[7.5 6],'y',y0+[-6 -6],'parent',ax,'arrow',0.5);
sysblock('position',[x0+4 y0-7 2 2],'name','ZOH',...
    'fontweight','bold','facecolor',[1 1 .9],'fontsize',8+2*isunix);
wire('x',x0+[4 2.5 2.5],'y',y0+[-6 -6 -0.5],'parent',ax,'arrow',0.5);

text(-2,11.5,'Digital Servo Loop','horiz','left','fontweight','bold','fontsize',14)
