%% script for Gibson's roll control system, problem 9.2
%% the plot for determining the LC condition is in figure(1)
%% the plot of the relay DF appears in figure(2), the Nyquist
%% Refer to my tutorial article in the EEE Encyclopedia
%% 
%% JH Taylor - 24 November 1995
%%% converted to SI units - 7 Apr 1999

close all; clear
h = 22.24; D = 444.8; % thruster "relay" constants (Newtons)
J = 4.68; % N-m/sec^2; = 3.45 lb-ft/sec^2
R0 = 0.6096; % m; = 2 ft - moment arm
Kh = 2000; % hydraulic servo amp DC gain
Kp = 1868; % N/rad; = 420 lb/rad - proportional control gain
Kv = 186.8; % N/(rad/sec); = 42 lb/(rad/sec) - derivative gain
RKoJ = R0*Kh/J;

num = [ RKoJ*Kv RKoJ*Kp ];   %% linear plant model
den = [ 1 120 2000 0 0 ];
% determine real axis crossing point
[Gm,Pm,Wcg,Wcp] = margin(num,den); Gco = -1/Gm,
w = 2.011*logspace(1,2,25);  %% w list => 3rd point intersects w. -1/DF
nyq4p2(num,den,w);
title('Gibson, Problem 9.2 (Missile Roll Control)');
text(-0.5,0.08,['G_{co} = ',num2str(Gco,4)]);
text(-0.5,0.06,['\omega_{co} = ',num2str(Wcg,4)]);
%% now evaluate DF and -1/DF (mDFinv):
for i=1:25,
   e(i) = 20*i+17.2;      %% => 18th point is on W(jw) plot
   DF(i) = (sqrt(e(i)*e(i)-h*h) - h*j)*4*D/(e(i)*e(i)*pi);
   mDFinv(i) = - pi*(sqrt(e(i)*e(i)-h*h) + h*j)/(4*D);
end;
% separate plot for the SIDF:
figure(2)
subplot(211); plot(e,real(DF)); grid
xlabel('Error Amplitude, e'); ylabel('Real Part of SIDF');
% plot and label the intersection point:
re18 = real(DF(18)); im18 = imag(DF(18));
hold on; plot(e(18),re18,'o');
text(0.97*e(18),1.5*re18,['Re N_s(E_{LC}) = ',num2str(re18)]);
title('Real & Imaginary Part of SIDF for Relay with Hysteresis')
axis([ 0 500 0 12])
subplot(212); plot(e,imag(DF)); grid
xlabel('Error Amplitude, e'); ylabel('Imaginary Part of SIDF');
hold on; plot(e(18),im18,'o');
mrdfi = -real(mDFinv(18)); midfi = -imag(mDFinv(18));
text(0.97*e(18),-1.2,['Im N_s(E_{LC}) = ',num2str(im18)]);
text(200,-5,['-1/N_s(E_{LC}) = - ',num2str(mrdfi),' - ',num2str(midfi),' j']);
axis([ 0 500 -9 0])
% print -deps 2409fg06
% OK - back to the Nyquist plot:
figure(1)
hold on;
%% re-do Nyquist plot of W with + markers and add -1/DF
[re,im,w] = nyquist(num,den,w);
plot(re,im,'+');
plot(real(mDFinv),imag(mDFinv),'.');
%%
wosc = w(3); Re = re(3); Im = im(3); %% recall 3rd point!
Wmag = sqrt(Re*Re + Im*Im);
%% transfer fcn from phi to e is Wmag*J*wosc*wosc/R0
gain = Wmag*J*wosc*wosc/R0
Edf = e(18) %% remember - 18th point!
Adf = Edf/gain;
text(-0.65,-.075,['\omega_{LC} = ',num2str(wosc,4)]);
text(-0.35,-.075,['G_{\phi} = ',num2str(gain,4),'  -> ']);
text(-0.65,-.10,['E_{LC} = ',num2str(Edf,4)]);
text(-0.35,-.10,['A_{LC} = ',num2str(Adf,4),'  rad. peak']);
%% plot([-1 0],[0 0],':');
hdl1 = plot(linspace(-1,0,100),0*ones(100),'.');
set(hdl1,'MarkerSize',3) 
plot([-.45 -.505],[.05 .005])
plot([-.656 -.60],[-.045 -.062])
%
% print -deps 2409fg07
