%DISKDEMO.M Demonstration design of harddisk digital controller.
echo on

% This file demonstrates MATLAB's ability for classical digital control
% system design by going through the design of a computer HARDDISK 
% read/write head position controller.

echo off
%	Copyright (c) 1986-93 by the MathWorks, Inc.
echo on

pause % Press any key to continue ...

% Using Newton's law, the simplest model for the read/write head has the 
% following differential equation:
%
%  I*theta_ddot + C*theta_dot + K*theta = Ki * i
%
%  where I is the inertia of the head assembly
%        C is the viscous damping coefficient of the bearings
%        K is the return spring constant
%        Ki is the motor torque constant
%        theta_ddot, theta_dot, and theta are the angular acceleration,
%          angular velocity and position of the head
%	i is the input current
%

pause % Press any key to continue ...

% Taking the laplace transform, the transfer function is:
%                Ki
%  H(s) = ----------------
%         I s^2 + C s + K
%
% Using the values I=.01 Kg m^2, C=.004 Nm/(rad/sed), K=10 Nm/rad,  and
% Ki=.05 Nm/rad form the transfer function description of this system

I = .01; C = 0.004; K = 10; Ki = .05;
NUM = [Ki];
DEN = [I C K];
printsys(NUM,DEN,'s');

pause % Press any key to continue ...

% Our task is to design a digital controller that can be used to provide 
% accurate positioning of the read/write head.  We will do the design in the
% digital domain.  

% First we must discretize our plant since it is continuous.  Since our
% plant will have a digital-to-analog-converter (with a zero-order hold)
% connected to its input, use the 'zoh' discretization method
% of the function C2DM. Use sample time Ts = 0.005  (5 ms)

Ts = 0.005;
w = logspace(0,3);
[mag,phase] = bode(NUM,DEN,w);
[num,den] = c2dm(NUM,DEN,Ts,'zoh');
[mzoh,pzoh,wzoh] = dbode(num,den,Ts);

% Now plot the results as a comparison.  Press any key after the plot ...
echo off
subplot(211)
semilogx(w,20*log10(mag),wzoh,20*log10(mzoh))
xlabel('Frequency (rad/sec)'), ylabel('Gain db')
title('c2d comparison plot')

subplot(212)
semilogx(w,phase,wzoh,pzoh)
xlabel('Frequency (rad/sec)'), ylabel('Phase deg')
pause % Press any key to continue ...
echo on

pause % Press any key to continue ...

% Now analyze the discrete system.
disp('Discrete system')
printsys(num,den,'z')

% Plot step response
subplot(111)
dstep(num,den); pause % Press any key after the plot ...

% The system oscillates quite a bit.  This is probably due to very light
% damping.  We can check this by computing the open loop eigenvalues.

disp('Open loop discrete eigenvalues'), ddamp(den,Ts); 
zgrid('new'), pzmap(1,den); pause % Press any key after the plot ...

% Note that the poles are very lightly damped and near the unit circle.
% We need to design a compensator that increases the damping of this system.

% Let's try to design a compensator.  The simplest compensator is a simple gain.
rlocus(num,den); hold off; pause % Press any key after the plot ...

% As shown in the root locus, the poles quickly leave the unit circle and go
% unstable.  We need to introduce some lead or a compensator with some zeros.
% Try the compensator:        K(z + a)
%                     D(z) = --------  where a < b
%                             (z + b)

pause % Press any key to continue ...

% Form compensator and connect in series with our plant
% Use a = -.85 and b = 0.
[numc,denc] = zp2tf([.85 ]',[0]',1);
[num2,den2] = series(numc,denc,num,den);

% Lets see how the frequency response has changed.
[mag,phase,w] = dbode(num,den,1);	% Use normalized frequency
[mag2,phase2] = dbode(num2,den2,1,w);

% Now plot a comparison plot.  Press any key after the plot ...
echo off
subplot(211), semilogx(w,20*log10(mag),w,20*log10(mag2))
xlabel('Frequency (rad/sec)'), ylabel('Gain dB')
subplot(212), semilogx(w,phase,w,phase2)
xlabel('Frequency (rad/sec)'), ylabel('Phase deg')
% Plot -180 degree line
hold on; semilogx([min(w(:)),max(w(:))],[-180,-180],'w--'); hold off;
pause % Press any key to continue ...
echo on

% So our compensator has indeed added lead.

% Now let's try the root locus again with our compensator
subplot(111)
zgrid('new'), rlocus(num2,den2); hold off; pause % Press any key after plot ...

% This time the poles stay within the unit circle for some time.
% Now its your turn, Using RLOCFIND chose the poles with greatest damping
% ratio.  (The lines drawn by ZGRID show the damping ratios from z=0 to 1
% in steps of .1)

pause % Press any key and then choose a point on the plot
[k,poles] = rlocfind(num2,den2);

disp(['You chose gain: ',num2str(k)]), ddamp(poles,Ts);

% Let's form the closed loop system so that we can analyze the design.
[numc,denc] = feedback(num2,den2,k,1);

% These eigenvalues should match the ones you chose.
disp('Closed loop eigenvalues'), ddamp(denc,Ts);

pause % Press any key to continue ...

% Closed loop time response
dstep(numc,denc); pause % Press any key after the plot ...

% So the response looks pretty good and settles in about 14 samples
% which is 14*Ts secs.

disp(['Our disc drive will have a seek time > ',num2str(14*Ts),' seconds.'])

pause % Press any key to continue ...

% Let's now look at the robustness of our design.  The most common classical
% robustness criteria is the gain and phase margin.  The criteria is determined
% by forming a unity feedback system, calculating the Bode response and looking
% for the phase and gain crossovers.  MATLAB contains a function MARGIN that
% determines the phase and gain margin given the Bode response.

% Form unity feedback system by connecting our design system with the gain
% we chose.  Leave the loop open so we can compute the open loop Bode response.
[num2,den2] = series(num2,den2,k,1);

% Compute Bode response and margins
[mag,phase,w] = dbode(num2,den2,Ts);
[Gm,Pm,Wcg,Wcp] = margin(mag,phase,w); 

% Plot Bode plot with margins
margin(mag,phase,w); pause % Press any key after the plot ...

% Gain margin db, @ frequency, Phase margin, @ frequency
Margins = [20*log10(Gm),Wcg,Pm,Wcp]

% Our design is robust and can tolerate a 10 db gain increase and a 40 degree
% phase lag without going unstable.  By continuing this design process we may
% be able to find a compensator that will stabilize the open loop system and 
% allow us to reduce the seek time (more damping would allow us to reduce the
% settling time in the step response).

echo off