# pvtol-nested.py - inner/outer design for vectored thrust aircraft
# RMM, 5 Sep 09
#
# This file works through a fairly complicated control design and
# analysis, corresponding to the planar vertical takeoff and landing
# (PVTOL) aircraft in Astrom and Mruray, Chapter 11.  It is intended
# to demonstrate the basic functionality of the python-control
# package.
#

from __future__ import print_function
from matplotlib.pyplot import * # Grab MATLAB plotting functions
from control.matlab import *    # MATLAB-like functions
import numpy as np

# System parameters
m = 4;				# mass of aircraft
J = 0.0475;			# inertia around pitch axis
r = 0.25;			# distance to center of force
g = 9.8;			# gravitational constant
c = 0.05;	 		# damping factor (estimated)

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

# Use state space versions
Pi = tf2ss(Pi);
Po = tf2ss(Po);

#
# 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); bode(Pi);

# Bode plot for the loop transfer function, with margins
figure(2); bode(Li);

# Compute the sensitivity and complementary sensitivity functions
Si = feedback(1, Li);
Ti = Li * Si;

# 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); bode(Hi)

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

figure(5); bode(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
#! Not yet implemented
# (gm, pm, wgc, wpc) = margin(L); 

#! TODO: this figure has something wrong; axis limits mismatch
figure(6); bode(L)

# Add crossover line
subplot(211); hold(True);
loglog([1e-4, 1e3], [1, 1], 'k-')

# Replot phase starting at -90 degrees
bode(L, logspace(-4, 3));
(mag, phase, w) = freqresp(L, logspace(-4, 3));
phase = phase - 360;
subplot(212);
semilogx([1e-4, 1e3], [-180, -180], 'k-')
hold(True);
semilogx(w, np.squeeze(phase), 'b-')
axis([1e-4, 1e3, -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;
axis([-700, 5300, -3000, 3000]); hold(True);
nyquist(L, (0.0001, 1000));
axis([-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-')

# Expanded region  
figure(8); clf; subplot(231); 
axis([-10, 5, -20, 20]); hold(True);
nyquist(L);
axis([-10, 5, -20, 20]);

# set up the color
color = 'b';

figure(9); 
(Yvec, Tvec) = step(T, linspace(0, 20));
plot(Tvec.T, Yvec.T); hold(True);

(Yvec, Tvec) = step(Co*S, linspace(0, 20));
plot(Tvec.T, Yvec.T);

figure(10); clf();
(P, Z) = pzmap(T, Plot=True)
print("Closed loop poles and zeros: ", P, Z)

# Gang of Four
figure(11); clf();
gangof4(Hi*Po, Co);