new pid-designer, built on sisotool, for manual tuning of a PID controller

sawyerbfuller · sawyerbfuller · commit 326364658893 · 2021-11-05T14:34:53.000-07:00
diff --git a/control/sisotool.py b/control/sisotool.py
@@ -1,11 +1,15 @@
-__all__ = ['sisotool']
+__all__ = ['sisotool', 'pid_designer']
 
 from control.exception import ControlMIMONotImplemented
 from .freqplot import bode_plot
 from .timeresp import step_response
 from .lti import issiso, isdtime
-from .xferfcn import TransferFunction
+from .xferfcn import tf
+from .statesp import ss
 from .bdalg import append, connect
+from .iosys import tf2io, ss2io, summing_junction, interconnect
+from control.statesp import _convert_to_statespace
+from control.lti import common_timebase, isctime
 import matplotlib
 import matplotlib.pyplot as plt
 import warnings
@@ -176,3 +180,139 @@ def _SisotoolUpdate(sys, fig, K, bode_plot_params, tvect=None):
     fig.subplots_adjust(top=0.9,wspace = 0.3,hspace=0.35)
     fig.canvas.draw()
 
+def pid_designer(plant, gain='P', sign=+1, input_signal='r',
+                           Kp0=0, Ki0=0, Kd0=0, tau=0.01,
+                           C_ff=0, derivative_in_feedback_path=False):
+    """Manual PID controller design using sisotool
+
+    Uses `Sisotool` to investigate the effect of adding or subtracting an
+    amount `deltaK` to the proportional, integral, or derivative (PID) gains of
+    a controller. One of the PID gains, `Kp`, `Ki`, or `Kd`, respectively, can
+    be modified at a time. `Sisotool` plots the step response, frequency
+    response, and root locus.
+
+    When first run, `deltaK` is set to 1; click on a branch of the root locus
+    plot to try a different value. Each click updates plots and prints
+    the corresponding `deltaK`. To tune all three PID gains, repeatedly call
+    `pid_designer`, and select a different `gain` each time (`'P'`, `'I'`,
+    or `'D'`). Make sure to add the resulting `deltaK` to your chosen initial
+    gain on the next iteration.
+
+    Example: to examine the effect of varying `Kp` starting from an intial
+    value of 10, use the arguments `gain='P', Kp0=10`. Suppose a `deltaK`
+    value of 5 gives satisfactory performance. Then on the next iteration,
+    to tune the derivative gain, use the arguments `gain='D', Kp0=15`.
+
+    By default, all three PID terms are in the forward path C_f in the diagram
+    shown below, that is,
+
+    C_f = Kp + Ki/s + Kd*s/(tau*s + 1).
+
+    If `plant` is a discrete-time system, then the proportional, integral, and
+    derivative terms are given instead by Kp, Ki*dt/2*(z+1)/(z-1), and
+    Kd/dt*(z-1)/z, respectively.
+
+        ------> C_ff ------    d
+        |                 |    |
+    r   |     e           V    V  u         y
+    ------->O---> C_f --->O--->O---> plant --->
+            ^-            ^-                |
+            |             |                 |
+            |             ----- C_b <-------|
+            ---------------------------------
+
+    It is also possible to move the derivative term into the feedback path
+    `C_b` using `derivative_in_feedback_path=True`. This may be desired to
+    avoid that the plant is subject to an impulse function when the reference
+    `r` is a step input. `C_b` is otherwise set to zero.
+
+    If `plant` is a 2-input system, the disturbance `d` is fed directly into
+    its second input rather than being added to `u`.
+
+    Remark: It may be helpful to zoom in using the magnifying glass on the
+    plot. Just ake sure to deactivate magnification mode when you are done by
+    clicking the magnifying glass. Otherwise you will not be able to be able to choose
+    a gain on the root locus plot.
+
+    Parameters
+    ----------
+    plant : :class:`LTI` (:class:`TransferFunction` or :class:`StateSpace` system)
+        The dynamical system to be controlled
+    gain : string (optional)
+        Which gain to vary by deltaK. Must be one of 'P', 'I', or 'D'
+        (proportional, integral, or derative)
+    sign : int (optional)
+        The sign of deltaK gain perturbation
+    input : string (optional)
+        The input used for the step response; must be 'r' (reference) or
+        'd' (disturbance) (see figure above)
+    Kp0, Ki0, Kd0 : float (optional)
+        Initial values for proportional, integral, and derivative gains,
+        respectively
+    tau : float (optional)
+        The time constant associated with the pole in the continuous-time
+        derivative term. This is required to make the derivative transfer
+        function proper.
+    C_ff : float or :class:`LTI` system (optional)
+        Feedforward controller. If :class:`LTI`, must have timebase that is
+        compatible with plant.
+    """
+    plant = _convert_to_statespace(plant)
+    if plant.ninputs == 1:
+        plant = ss2io(plant, inputs='u', outputs='y')
+    elif plant.ninputs == 2:
+        plant = ss2io(plant, inputs=('u', 'd'), outputs='y')
+    else:
+        raise ValueError("plant must have one or two inputs")
+    #plant = ss2io(plant, inputs='u', outputs='y')
+    C_ff = ss2io(_convert_to_statespace(C_ff),   inputs='r', outputs='uff')
+    dt = common_timebase(plant, C_ff)
+
+    # create systems used for interconnections
+    e_summer = summing_junction(['r', '-y'], 'e')
+    if plant.ninputs == 2:
+        u_summer = summing_junction(['ufb', 'uff'], 'u')
+    else:
+        u_summer = summing_junction(['ufb', 'uff', 'd'], 'u')
+
+    prop  = tf(1,1)
+    if isctime(plant):
+        integ = tf(1,[1, 0])
+        deriv = tf([1, 0], [tau, 1])
+    else:
+        integ = tf([dt/2, dt/2],[1, -1], dt)
+        deriv = tf([1, -1],[dt, 0], dt)
+
+    # add signal names
+    prop  = tf2io(prop,        inputs='e', outputs='prop_e')
+    integ = tf2io(integ,       inputs='e', outputs='int_e')
+    if derivative_in_feedback_path:
+        deriv = tf2io(-deriv,  inputs='y', outputs='deriv_')
+    else:
+        deriv = tf2io(deriv,   inputs='e', outputs='deriv_')
+
+    # create gain blocks
+    Kpgain = tf2io(tf(Kp0, 1),            inputs='prop_e',  outputs='ufb')
+    Kigain = tf2io(tf(Ki0, 1),            inputs='int_e',   outputs='ufb')
+    Kdgain = tf2io(tf(Kd0, 1),            inputs='deriv_',  outputs='ufb')
+
+    # for the gain that is varied, create a special gain block with an
+    # 'input' and an 'output' signal to create the loop transfer function
+    if gain in ('P', 'p'):
+        Kpgain = ss2io(ss([],[],[],[[0, 1], [-sign, Kp0]]),
+            inputs=['input', 'prop_e'], outputs=['output', 'ufb'])
+    elif gain in ('I', 'i'):
+        Kigain = ss2io(ss([],[],[],[[0, 1], [-sign, Ki0]]),
+            inputs=['input', 'int_e'],  outputs=['output', 'ufb'])
+    elif gain in ('D', 'd'):
+        Kdgain = ss2io(ss([],[],[],[[0, 1], [-sign, Kd0]]),
+            inputs=['input', 'deriv_'], outputs=['output', 'ufb'])
+    else:
+        raise ValueError(gain + ' gain not recognized.')
+
+    # the second input and output are used by sisotool to plot step response
+    loop = interconnect((plant, Kpgain, Kigain, Kdgain, prop, integ, deriv,
+                            C_ff, e_summer, u_summer),
+                            inplist=['input', input_signal], outlist=['output', 'y'])
+    sisotool(loop)
+    return loop[1, 1]
diff --git a/control/tests/sisotool_test.py b/control/tests/sisotool_test.py
@@ -6,11 +6,11 @@
 from numpy.testing import assert_array_almost_equal
 import pytest
 
-from control.sisotool import sisotool
+from control.sisotool import sisotool, pid_designer
 from control.rlocus import _RLClickDispatcher
 from control.xferfcn import TransferFunction
 from control.statesp import StateSpace
-
+from control import c2d
 
 @pytest.mark.usefixtures("mplcleanup")
 class TestSisotool:
@@ -140,3 +140,25 @@ def test_sisotool_mimo(self,  sys222, sys221):
         # but 2 input, 1 output should
         with pytest.raises(ControlMIMONotImplemented):
             sisotool(sys221)
+
+@pytest.mark.usefixtures("mplcleanup")
+class TestPidDesigner:
+    syscont = TransferFunction(1,[1, 3, 0])
+    sysdisc1 = c2d(TransferFunction(1,[1, 3, 0]), .1)
+    syscont221 = StateSpace([[-.3, 0],[1,0]],[[-1,],[.1,]], [0, -.3], 0)
+
+    # cont or discrete, vary P I or D
+    @pytest.mark.parametrize('plant', (syscont, sysdisc1))
+    @pytest.mark.parametrize('gain', ('P', 'I', 'D'))
+    @pytest.mark.parametrize("kwargs", [{'Kp0':0.01},])
+    def test_pid_designer_1(self, plant, gain, kwargs):
+        pid_designer(plant, gain, **kwargs)
+
+    # input from reference or disturbance
+    @pytest.mark.parametrize('plant', (syscont, syscont221))
+    @pytest.mark.parametrize("kwargs", [
+        {'input_signal':'r', 'Kp0':0.01, 'derivative_in_feedback_path':True},
+        {'input_signal':'d', 'Kp0':0.01, 'derivative_in_feedback_path':True},])
+    def test_pid_designer_2(self, plant, kwargs):
+        pid_designer(plant, **kwargs)
+
