% pvtol_nested.m - inner/outer design for vectored thrust aircraft
% RMM, 19 Nov 02
%
% This file contains the code corresponding to CDS 110 Lecture 8.2
%
% Note that we include a small damping factor in the dynamics to keep
% MATLAB happy (it doesn't like pure imaginary poles).

aminit;
amuse('pvtol');
pvtol_params;			% System parameters

% Transfer functions for dynamics
Pi = tf([r], [J 0 0]);			% inner loop (roll)
Po = tf([1], [m c 0]);			% outer loop (position)

%
% Inner loop control design
%
% This is the controller for the pitch dynamics.  Goal is to have
% fast response for the pitch dynamics so that we can use this as a 
% control for the lateral dynamics
%

% Design a simple lead controller for the system
k = 200;  a = 2;  b = 50;
Ci = k*tf([1 a], [1 b]);		% lead compensator
Li = Pi*Ci;

% Bode plot for the open loop process
figure(1); clf; subplot(221); 
[magh, phaseh] = ambode(Pi);
subplot(phaseh); amaxis([0.1 10 -190 0]);

% Bode plot for the loop transfer function, with margins
figure(2); clf; subplot(221);
[magh, phaseh] = ambode(Li);
[gm, pm, wcg, wcp] = margin(Li);

% Compute the sensitivity and complementary sensitivity functions
Si = 1/(1+Li);
Ti = Li/(1+Li);

% Check to make sure that the specification is met
figure(3);  gangof4(Pi, Ci);

% Compute out the actual transfer function from u1 to v1 (see L8.2 notes)
% Hi = Ci*(1-m*g*Pi)/(1+Ci*Pi);
Hi = parallel(feedback(Ci, Pi), -m*g*feedback(Ci*Pi, 1));

figure(4); clf; subplot(221);
ambode(Hi);

% Now design the lateral control system
a = 0.3; b = 10; K = 2;
Co = -K*tf([1 a], [1 b]);		% another lead compensator
Lo = -m*g*Po*Co;

figure(5); clf; subplot(221);
  margin(Lo);

% Finally compute the real outer-loop loop gain + responses
L = Co*Hi*Po;
S = feedback(1, L);
T = feedback(L, 1);

% Compute stability margins
[gm, pm, wgc, wpc] = margin(L); 

figure(6); clf; subplot(221);
[magh, phaseh] = ambode(L);

% Add crossover line
subplot(magh); hold on;
loglog([10^-4 10^3], [1 1], 'k-', 'LineWidth', AM_data_linewidth);

% Replot phase starting at -90 degrees
[mag, phase, w] = bode(L, {10^-4, 10^3});
phase = phase - 360;
subplot(phaseh);
semilogx([10^-4 10^3], [-180 -180], 'k-', 'LineWidth', AM_data_linewidth);
hold on;
semilogx(w, phase(:), 'b-', 'LineWidth', AM_data_linewidth);
amaxis([10^-4 10^3 -360 0]);
xlabel('Frequency [deg]'); ylabel('Phase [deg]');
set(gca, 'YTick', [-360 -270 -180 -90 0]);
set(gca, 'XTick', [10^-4 10^-2 1 100]);

%
% Nyquist plot for complete design
%
figure(7); clf; subplot(231); 
amaxis([-700 5300 -3000 3000]); hold on;
amnyquist(L, {0.0001 1000});
amaxis([-700 5300 -3000 3000]);

% Add a box in the region we are going to expand
plot([-400 -400 200 200 -400], [-100 100 100 -100 -100], 'r-', ...
  'LineWidth', AM_ref_linewidth);

% Expanded region  
figure(8); clf; subplot(231); 
amaxis([-10 5 -20 20]); hold on;
AM_nyquist_plain = 1;
amnyquist(L);
AM_nyquist_plain = 0;
amaxis([-10 5 -20 20]);

% set up the color
color = 'b';

% Add arrows to the plot
H1 = freqresp(L, 0.4); H2 = freqresp(L, 0.41);
arrow([real(H1), imag(H1)], [real(H2), imag(H2)], AM_normal_arrowsize, ...
  'EdgeColor', color, 'FaceColor', color);

H1 = freqresp(L, 0.35); H2 = freqresp(L, 0.36);
arrow([real(H2), -imag(H2)], [real(H1), -imag(H1)], AM_normal_arrowsize, ...
  'EdgeColor', color, 'FaceColor', color);

figure(9); 
step(T, 20); hold on;
step(Co*S, 20);

fprintf(1, 'Closed loop poles and zeros:\n');  
[P, Z] = pzmap(T)

% Gang of Four
figure(10); clf; 

gangof4(Hi*Po, Co, {0.01 1000});
