echo off
% DISKDEMO demonstrates MATLAB's ability for classical digital control
% system design by going through the design of a computer hard disk 
% read/write head position controller.

%   Copyright (c) 1986-1999 The Mathworks, Inc. All Rights Reserved.
%   $Revision: 1.7 $  $Date: 1999/01/05 15:21:12 $
clc
clf
help diskdemo
echo on
pause % Strike any key to continue.
clc
% Using Newton's law, the simplest model for the read/write head has the 
% following differential equation:
%
%  J*theta_ddot + C*theta_dot + K*theta = Ki * i
%
%  where J 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) = ----------------
%         J s^2 + C s + K
%
% Using the values J=.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

J = .01; C = 0.004; K = 10; Ki = .05;
num = Ki;
den = [J C K];
H = tf(num,den)

pause % Press any key to continue ...
clc
% 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 C2D.  Use sample time Ts = 0.005  (5 ms)

Ts = 0.005;
Hd = c2d(H,Ts,'zoh')

% Compare the Bode plots of the continuous and discretized systems:
bode(H,'-',Hd,'--')

pause % Press any key to continue ...

% Now analyze the discrete system: plot step response
step(Hd), 

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.

% Open loop discrete eigenvalues
damp(Hd); 
zgrid('new'), pzmap(Hd), hold off

% 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.

pause % Press any key after the plot ...
clc
% Let's try to design a compensator.  The simplest compensator is a simple gain.
rlocus(Hd), zgrid, set(gca,'xlim',[-1 1],'ylim',[-1.5,1.5]); 

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:        
%                            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.

D = zpk(0.85,0,1,Ts)
oloop = Hd * D;

% Lets see how the frequency response has changed. The open-loop response
% with the compensator is the solid line:
bode(Hd,'--',oloop,'-')

% So our compensator has indeed added lead.

pause % Press any key to continue ...
clc
% Now let's try the root locus again with our compensator
rlocus(oloop); zgrid, set(gca,'xlim',[-1 1],'ylim',[-1.5,1.5]); 

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 0.1)

pause % Press any key and then choose a point on the plot

[k,poles] = rlocfind(oloop);

echo off
fprintf('\n');
disp(['You chose gain: ',num2str(k)]), 
fprintf('\n');
disp('Yielding closed-loop poles at:')
echo on
ddamp(poles,Ts);

% Let's form the closed-loop system so that we can analyze the design.
cloop = feedback(oloop,k);

% These eigenvalues should match the ones you chose.

% Closed loop eigenvalues 
damp(cloop)

pause % Press any key to continue ...

% Plot the closed-loop step  response
step(cloop), 

pause % Press any key after the plot ...

% So the response looks pretty good and settles in about 14 samples
% which is 14*0.005 = 0.07 second.

echo off
fprintf('\n');
disp(['Our disc drive will have a seek time > ',num2str(14*Ts),' seconds.'])
echo on

pause % Press any key to continue ...
clc
% Let's now look at the robustness of our design.  The most common classical
% robust robustness criteria are the gain and phase margins. Use the function
% MARGIN to compute these margins.

% First form the unity-feedback open loop by connecting our design system 
% with the gain k chosen above. 
olk = k * oloop;

% Compute the gain margin Gm, phase margin Pm (in degrees), and corresponding 
% crossover frequencies Wcg and Wcp (in radians/sec.):
[Gm,Pm,Wcg,Wcp] = margin(olk);
Margins = [Gm Wcg Pm Wcp]

% The gain margin in dB is:
20*log10(Gm)

% Plot Bode plot with margins
margin(olk), 

pause % Press any key after the plot ...

% 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
