# L9-1_loopsyn.py - plots for loop synthesis lecture
# RMM, 25 Nov 2012

import numpy as np
import matplotlib.pyplot as mpl
import control as ctrl

#
# Define a system, controller and loop transfer function to use for
# illustrating the frequency domain specifications
#

s = ctrl.tf([1, 0], [1])
P = 1/(s**2 + 0.5*s + 1) * 10/(s + 10)
C = 200 * (s + 1) / (s + 20)
L = P*C

# Plot the bode plot
mpl.figure(1); mpl.clf()
ctrl.bode(L, np.logspace(-1,2,100))
mpl.subplot(211); mpl.grid(None, axis='both', which='both')
mpl.subplot(212); mpl.grid(None, axis='both', which='both')

#
# Control design for a second order system using lead compensation
#

# Process dynamics
p1 = 1; p2 = 5                  # location of process poles
P = p1*p2 / ((s+p1) * (s+p2))   # process dynamics

# Controller specs
wgc = 100.                      # gain crossover frequency
a = wgc/5.; b = wgc*5.          # lead compensator break frequencies
k = 400.                        # zero frequency gain (assumes P(0) = 1)

# Lead compensator
C1 = k * (b/a) * (s+a) / (s+b)

# Plot out the open loop curves so that we can see what things look like
mpl.figure(2); mpl.clf()
W = np.logspace(-2, 4, 100)
ctrl.bode(P, W); mpl.savefig('secord_bode-1a.png')
ctrl.bode(C1, W); mpl.savefig('secord_bode-1b.png')
ctrl.bode(P*C1, W); 
mpl.subplot(211); mpl.legend(('P', 'C', 'PC'), loc='lower left')
mpl.savefig('secord_bode-1c.png')

# Check the gain and phase margin using a Nyquist plot
mpl.figure(3); mpl.clf()
ctrl.nyquist(P*C1)
mpl.savefig('secord_nyquist-1.png')

# Check the gang of 4 to make sure all other responses look OK
mpl.figure(4); mpl.clf()
ctrl.gangof4(P, C1)
mpl.savefig('secord_gof-1.png')

# Plot step responses
mpl.figure(5); i = 1
for sys in (1/(1 + P*C1), P/(1 + P*C1), C1/(1 + P*C1), P*C1/(1 + P*C1)):
    mpl.subplot(2, 2, i)
    t, y = ctrl.step_response(sys)
    mpl.plot(t, y)
    i = i + 1
mpl.savefig('secord_steps-1.png')

# Modify the controller to decrease the noise sensitivity
wgc = 50.                       # gain crossover frequency
a = wgc/5.; b = wgc*5.          # lead compensator break frequencies
k = 50.                         # zero frequency gain (assumes P(0) = 1)
C2 = k * (b/a) * (s+a) / (s+b) * 1 / (s/(b*4) + 1)

# Replot, Bode, Nyquist and Gang of 4
mpl.figure(6); mpl.clf(); ctrl.bode((P, C2, P*C2))
mpl.figure(7); mpl.clf(); ctrl.nyquist(P*C2)
mpl.figure(8); mpl.clf(); ctrl.gangof4(P, C2)
mpl.figure(9); i = 1
T = np.linspace(0,2,50)
for sys in (1/(1 + P*C2), P/(1 + P*C2), C2/(1 + P*C2), P*C2/(1 + P*C2)):
    mpl.subplot(2, 2, i)
    t, y = ctrl.step_response(sys, T)
    mpl.plot(t, y)
    lims = np.array(mpl.axis()); lims[1] = 2; mpl.axis(lims)
    i = i + 1

#
# Process inversion
#
# As a final controller, set up a desired loop transfer function and just
# invert out the reponse.  For the desired transfer function, us a single
# ingrator crossing over at about 10 rad/sec with a third order rolloff at
# 100 rad/sec

Ld = 10 / s / (s/100+1)**3
C3 = Ld / P

mpl.figure(11); mpl.clf(); ctrl.bode((P, C3, P*C3))
mpl.figure(10); mpl.clf(); ctrl.gangof4(P, C3)

# Show all of the figures 
mpl.show()