# type2_type3.py - demonstration for type2 versus type3 control comparing
#   tracking and disturbance rejection for two proposed controllers
# Gunnar Ristroph, 15 January 2010

import os
import matplotlib.pyplot as plt  # Grab MATLAB plotting functions
import control as ct
import numpy as np

integrator = ct.tf([0, 1], [1, 0])  # 1/s

# Parameters defining the system
J = 1.0
b = 10.0
Kp = 110.
Ki = Kp/2.
Kii = Ki

# Plant transfer function from torque to rate
inertia = integrator*1/J
friction = b  # transfer function from rate to torque
P = inertia  # friction is modelled as a separate block

# Gyro transfer function from rate to rate
gyro = 1.  # for now, our gyro is perfect

# Controller transfer function from rate error to torque
C_type2 = (1. + Ki*integrator)*Kp*1.5
C_type3 = (1. + Ki*integrator)*(1. + Kii*integrator)*Kp

# System Transfer Functions
# tricky because the disturbance (base motion) is coupled in by friction
closed_loop_type2 = ct.feedback(C_type2*ct.feedback(P, friction), gyro)
disturbance_rejection_type2 = P*friction/(1. + P*friction+P*C_type2)
closed_loop_type3 = ct.feedback(C_type3*ct.feedback(P, friction), gyro)
disturbance_rejection_type3 = P*friction/(1. + P*friction + P*C_type3)

# Bode plot for the system
plt.figure(1)
ct.bode(closed_loop_type2, np.logspace(0, 2)*2*np.pi, dB=True, Hz=True)  # blue
ct.bode(closed_loop_type3, np.logspace(0, 2)*2*np.pi, dB=True, Hz=True)  # green
plt.show(block=False)

plt.figure(2)
ct.bode(disturbance_rejection_type2, np.logspace(0, 2)*2*np.pi, Hz=True)  # blue
ct.bode(disturbance_rejection_type3, np.logspace(0, 2)*2*np.pi, Hz=True)  # green

if 'PYCONTROL_TEST_EXAMPLES' not in os.environ:
    plt.show()