diff --git a/control/rlocus.py b/control/rlocus.py index ee30fe489..4b1af57f7 100644 --- a/control/rlocus.py +++ b/control/rlocus.py @@ -168,8 +168,8 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None, else: if ax is None: ax = plt.gca() - fig = ax.figure - ax.set_title('Root Locus') + fig = ax.figure + ax.set_title('Root Locus') if print_gain and not sisotool: fig.canvas.mpl_connect( @@ -180,7 +180,7 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None, fig.axes[1].plot( [root.real for root in start_mat], [root.imag for root in start_mat], - marker='s', markersize=8, zorder=20, label='gain_point') + marker='s', markersize=6, zorder=20, color='k', label='gain_point') s = start_mat[0][0] if isdtime(sys, strict=True): zeta = -np.cos(np.angle(np.log(s))) @@ -188,7 +188,7 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None, zeta = -1 * s.real / abs(s) fig.suptitle( "Clicked at: %10.4g%+10.4gj gain: %10.4g damp: %10.4g" % - (s.real, s.imag, 1, zeta), + (s.real, s.imag, kvect[0], zeta), fontsize=12 if int(mpl.__version__[0]) == 1 else 10) fig.canvas.mpl_connect( 'button_release_event', @@ -623,7 +623,7 @@ def _RLFeedbackClicksPoint(event, sys, fig, ax_rlocus, sisotool=False): ax_rlocus.plot( [root.real for root in mymat], [root.imag for root in mymat], - marker='s', markersize=8, zorder=20, label='gain_point') + marker='s', markersize=6, zorder=20, label='gain_point', color='k') else: ax_rlocus.plot(s.real, s.imag, 'k.', marker='s', markersize=8, zorder=20, label='gain_point') @@ -769,7 +769,7 @@ def _default_wn(xloc, yloc, max_lines=7): """ sep = xloc[1]-xloc[0] # separation between x-ticks - + # Decide whether to use the x or y axis for determining wn if yloc[-1] / sep > max_lines*10: # y-axis scale >> x-axis scale diff --git a/control/sisotool.py b/control/sisotool.py index 18c3b5d12..5accd1453 100644 --- a/control/sisotool.py +++ b/control/sisotool.py @@ -1,11 +1,15 @@ -__all__ = ['sisotool'] +__all__ = ['sisotool', 'rootlocus_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, StateSpace +from control.lti import common_timebase, isctime import matplotlib import matplotlib.pyplot as plt import warnings @@ -176,3 +180,156 @@ 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() +# contributed by Sawyer Fuller, minster@uw.edu 2021.11.02, based on +# an implementation in Matlab by Martin Berg. +def rootlocus_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, + plot=True): + """Manual PID controller design based on root locus 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 0; 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 + `rootlocus_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. + derivative_in_feedback_path : bool (optional) + Whether to place the derivative term in feedback transfer function + `C_b` instead of the forward transfer function `C_f`. + plot : bool (optional) + Whether to create Sisotool interactive plot. + + Returns + ---------- + closedloop : class:`StateSpace` system + The closed-loop system using initial gains. + """ + + 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") + 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') + + if isctime(plant): + prop = tf(1, 1) + integ = tf(1, [1, 0]) + deriv = tf([1, 0], [tau, 1]) + else: # discrete-time + prop = tf(1, 1, dt) + integ = tf([dt/2, dt/2], [1, -1], dt) + deriv = tf([1, -1], [dt, 0], dt) + + # add signal names by turning into iosystems + 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, replace gain block with a special block + # that has an 'input' and an 'output' that creates 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']) + if plot: + sisotool(loop, kvect=(0.,)) + cl = loop[1, 1] # closed loop transfer function with initial gains + return StateSpace(cl.A, cl.B, cl.C, cl.D, cl.dt) diff --git a/control/tests/lti_test.py b/control/tests/lti_test.py index 7e4f0ddb4..e2f7f2e03 100644 --- a/control/tests/lti_test.py +++ b/control/tests/lti_test.py @@ -77,7 +77,7 @@ def test_damp(self): p2_splane = -wn2 * zeta2 + 1j * wn2 * np.sqrt(1 - zeta2**2) p2_zplane = np.exp(p2_splane * dt) np.testing.assert_almost_equal(p2, p2_zplane) - + def test_dcgain(self): sys = tf(84, [1, 2]) np.testing.assert_allclose(sys.dcgain(), 42) @@ -136,7 +136,7 @@ def test_common_timebase(self, dt1, dt2, expected, sys1, sys2): (0, 1), (1, 2)]) def test_common_timebase_errors(self, i1, i2): - """Test that common_timbase throws errors on invalid combinations""" + """Test that common_timbase raises errors on invalid combinations""" with pytest.raises(ValueError): common_timebase(i1, i2) # Make sure behaviour is symmetric diff --git a/control/tests/sisotool_test.py b/control/tests/sisotool_test.py index ab5d546dd..fb2ac46e5 100644 --- 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, rootlocus_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,40 @@ 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: + @pytest.fixture + def plant(self, request): + plants = { + '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)} + return plants[request.param] + + # test permutations of system construction without plotting + @pytest.mark.parametrize('plant', ('syscont', 'sysdisc1', 'syscont221'), indirect=True) + @pytest.mark.parametrize('gain', ('P', 'I', 'D')) + @pytest.mark.parametrize('sign', (1,)) + @pytest.mark.parametrize('input_signal', ('r', 'd')) + @pytest.mark.parametrize('Kp0', (0,)) + @pytest.mark.parametrize('Ki0', (1.,)) + @pytest.mark.parametrize('Kd0', (0.1,)) + @pytest.mark.parametrize('tau', (0.01,)) + @pytest.mark.parametrize('C_ff', (0, 1,)) + @pytest.mark.parametrize('derivative_in_feedback_path', (True, False,)) + @pytest.mark.parametrize("kwargs", [{'plot':False},]) + def test_pid_designer_1(self, plant, gain, sign, input_signal, Kp0, Ki0, Kd0, tau, C_ff, + derivative_in_feedback_path, kwargs): + rootlocus_pid_designer(plant, gain, sign, input_signal, Kp0, Ki0, Kd0, tau, C_ff, + derivative_in_feedback_path, **kwargs) + + # test creation of sisotool plot + # input from reference or disturbance + @pytest.mark.parametrize('plant', ('syscont', 'syscont221'), indirect=True) + @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): + rootlocus_pid_designer(plant, **kwargs) + diff --git a/doc/control.rst b/doc/control.rst index a3e28881b..4b18cfb53 100644 --- a/doc/control.rst +++ b/doc/control.rst @@ -124,6 +124,7 @@ Control system synthesis lqe mixsyn place + rlocus_pid_designer Model simplification tools ==========================