Ejemplo n.º 1
0
    def plot_bode(self, fig):
        tf = self.tf if self.loop_type == 'ol' else self.tf_plant * self.tf_comp

        if self.settings.is_bode_default():
            mag, phase, omega = control.bode(tf, dB=True, Plot=False)
        else:
            fmin = self.settings.get_bode_freq_min()
            fmax = self.settings.get_bode_freq_max()
            mag, phase, omega = control.bode(tf,
                                             omega_limits=[fmin, fmax],
                                             dB=True,
                                             Plot=False)

        mag = 20 * np.log10(mag)
        phase = phase * 180.0 / np.pi

        fig.clf()

        ax1 = fig.add_subplot(2, 1, 1)
        ax1.semilogx(omega, mag)
        ax1.grid(which="both")
        ax1.set_xlabel('Frequency (rad/s)')
        ax1.set_ylabel('Magnitude (dB)')

        ax2 = fig.add_subplot(2, 1, 2)
        ax2.semilogx(omega, phase)
        ax2.grid(which="both")
        ax2.set_xlabel('Frequency (rad/s)')
        ax2.set_ylabel('Phase (deg)')

        return fig
Ejemplo n.º 2
0
def plot_loops(name, G_ol, G_cl):
    # type: (str, control.tf, control.tf) -> None
    """
    Plot loops
    :param name: Name of axis
    :param G_ol: open loop transfer function
    :param G_cl: closed loop transfer function
    """
    plt.figure()
    plt.plot(*control.step_response(G_cl, np.linspace(0, 1, 1000)))
    plt.title(name + ' step response')
    plt.grid()

    plt.figure()
    control.bode(G_ol)
    print('margins', control.margin(G_ol))
    plt.subplot(211)
    plt.title(name + ' open loop bode plot')

    plt.figure()
    control.rlocus(G_ol, np.logspace(-2, 0, 1000))
    for pole in G_cl.pole():
        plt.plot(np.real(pole), np.imag(pole), 'rs')
    plt.title(name + ' root locus')
    plt.grid()
Ejemplo n.º 3
0
def plot_attitude_rate_design(name, G_ol, G_cl):
    import matplotlib.pyplot as plt
    plt.figure()
    plt.plot(*control.step_response(G_cl, np.linspace(0, 1, 1000)))
    plt.title(name + ' rate step resposne')

    plt.figure()
    control.bode(G_ol)
    print(control.margin(G_ol))

    plt.figure()
    control.rlocus(G_ol, np.logspace(-2, 0, 1000))
    for pole in G_cl.pole():
        plt.plot(np.real(pole), np.imag(pole), 'rs')
    plt.title(name + ' rate step root locus')
    def Bode_Margin(sys, omega):
        w = omega
        mag, phaserad, w = ctrl.bode(sys, w)
        magdb = 20 * log(mag)
        phasedeg = phaserad * 180 / pi
        gm, pm, wg, wp = ctrl.margin(sys)

        a1 = f.add_subplot(221)
        a1.set_xscale("log")
        a1.plot(w, magdb)
        a1.plot(wp, 0, '.y')
        a1.text(wp, 0 + 25, "wc=%.2f" % wp)
        # a1.title = "Bode"
        # a1.ylabel = "Magnitude dB "
        # a1.xlabel = "Angular frequency"

        a2 = f.add_subplot(223)
        a2.set_xscale("log")
        a2.plot(w, phasedeg)
        a2.plot(wp, pm - 180, '.y')
        a2.text(wp, pm - 180 + 25, "phase=%.2f" % (pm - 180))
        # a2.ylabel = "Phase (degree) "
        # a2.xlabel = "Angular frequency"

        a3 = f.add_subplot(122)
        sysF = sys / (1 + sys)
        T = np.arange(0, 30, 0.5)
        T, yout = ctrl.step_response(sysF, T)
        a3.plot(T, yout)
        # a3.title = "step response"
        # a3.xlabel = "time"
        # a3.ylabel = "Magnitude"

        return wp, pm
Ejemplo n.º 5
0
def zpk(z, p, k):
    Gs = control.tf(36, [1, 3.6, 0])
    # Define Compensator transfer function
    num, den = matlab.zpk2tf(z, p, k)
    Ds = matlab.tf(num, den)
    print(Ds)

    # Draw the open-loop frequency resp. for the comp. sys
    DsGs = Ds*Gs
    gm, pm, wg, wp = control.margin(DsGs)
    print(f"Gain margin = {gm} dB")
    print(f"Phase margin = {round(pm, 2)} degrees")
    print(f"Frequency for Gain Margin = {wg} radians/sec")
    print(f"Frequecny for Phase Margin = {wp} radians/sec")
    omega_comp = np.logspace(-2,2,2000)
    mag_comp, phase_comp, omega_comp = control.bode(DsGs, omega=omega_comp)
    mag_comp = 20 * np.log10(mag_comp)
    phase_comp = np.degrees(phase_comp)
    omega_comp = omega_comp.T
    phase_comp = phase_comp.T
    mag_comp = mag_comp.T

    omega_comp = list(omega_comp)
    phase_comp = list(phase_comp)
    mag_comp = list(mag_comp)

    return omega_comp, mag_comp, phase_comp, gm, pm, wp, wg 
Ejemplo n.º 6
0
    def PI_analyze(self, system, controller):
        self.S = system
        self.C = controller

        self.C.sysOpenCE(self.S.CE)
        print("Open Loop: %s" % self.C.sysOCE)
        self.C.sysClosedCE()
        print("Closed Loop: %s" % self.C.sysCCE)

        self.C.sysRecomp(P=10, I=30)
        OC1 = self.C.sysOpenCE(self.S.CE)
        self.C.sysRecomp(P=50, I=150)
        OC2 = self.C.sysOpenCE(self.S.CE)
        self.C.sysRecomp(P=200, I=600)
        OC3 = self.C.sysOpenCE(self.S.CE)
        control.bode([OC1, OC2, OC3])
Ejemplo n.º 7
0
 def plot(self):
     G = control.TransferFunction((1, 1.5), (1, 11, 10, 0))
     mag, phase, omega = control.bode(G)            
     plt.title('Bode Plots', y= 2.20) 
     plt.gcf().canvas.set_window_title('Bode Plots')
     plt.grid()
     plt.show()
Ejemplo n.º 8
0
def pid(Kp, Ki, Kd):
    # PID Controller
    s = matlab.tf('s')
    Ds = Kp + Ki/s + Kd*s
    # Draw the open-loop frequency resp. for the comp. sys
    Gs = control.tf(36, [1, 3.6, 0])
    DsGs = Ds*Gs
    gm, pm, wg, wp = control.margin(DsGs)
    print(f"Gain margin = {gm} dB")
    print(f"Phase margin = {round(pm, 2)} degrees")
    print(f"Frequency for Gain Margin = {wg} radians/sec")
    print(f"Frequecny for Phase Margin = {wp} radians/sec")
    omega_comp = np.logspace(-2,2,2000)
    mag_comp, phase_comp, omega_comp = control.bode(DsGs, omega=omega_comp)
    mag_comp = 20 * np.log10(mag_comp)
    phase_comp = np.degrees(phase_comp)
    omega_comp = omega_comp.T
    phase_comp = phase_comp.T
    mag_comp = mag_comp.T

    omega_comp = list(omega_comp)
    phase_comp = list(phase_comp)
    mag_comp = list(mag_comp)

    return omega_comp, mag_comp, phase_comp, gm, pm, wp, wg 
Ejemplo n.º 9
0
def bode_pid(sys, Kp, Ki, Kd):
    # open-loop system transfer function
    try:
        num, den = model(sys)
    except:
        # for error detection
        print("Err: system in not defined")
        return
    Gs = control.tf(num, den)

    # PID Controller
    s = matlab.tf('s')
    Ds = Kp + Ki/s + Kd*s

    # Compensated open-loop transfer function
    DsGs = Ds*Gs

    # bode plot arrays
    omega = np.logspace(-2, 2, 2000)
    mag, phase, omega = control.bode(DsGs, omega=omega)
    mag = 20 * np.log10(mag)  # mag in db
    phase = np.degrees(phase)  # phase in degrees

    # convert numpy arrays to lists
    omega = list(omega)
    phase = list(phase)
    mag = list(mag)

    # round lists to 6 decimal floating digits
    ndigits = 6
    omega = [round(num, ndigits) for num in omega]
    phase = [round(num, ndigits) for num in phase]
    mag = [round(num, ndigits) for num in mag]

    return omega, mag, phase
Ejemplo n.º 10
0
def Gs():
    # Define plant transfer function
    Gs = control.tf(36, [1, 3.6, 0])
    [n, d] = control.tfdata(Gs)
    b = str(control.tf(d,1 ))
    print(b[5])
    num = "36"
    den = "s^2 + 3.6s"
    # Draw open-loop frequency response for the planet
    gm, pm, wg, wp = control.margin(Gs)
    # print(f"Gain margin = {gm} dB")
    # print(f"Phase margin = {round(pm, 2)} degrees")
    # print(f"Frequency for Gain Margin = {wg} radians/sec")
    # print(f"Frequecny for Phase Margin = {wp} radians/sec")
    omega = np.logspace(-2,2,2000)
    mag, phase, omega = control.bode(Gs, omega=omega)
    mag = 20 * np.log10(mag)
    phase= np.degrees(phase)
    omega = omega.T
    phase = phase.T
    mag = mag.T

    omega= list(omega)
    phase= list(phase)
    mag= list(mag)

    #mag = 20*np.log(mag,10)
    return  num, den, omega, mag, phase,gm, pm, wg, wp
Ejemplo n.º 11
0
def bode_sys(sys):
    # open-loop system transfer function
    try:
        num, den = model(sys)
    except:
        # for error detection
        print("Err: system in not defined")
        return
    Gs = control.tf(num, den)

    # bode plot arrays
    omega = np.logspace(-2, 2, 2000)
    mag, phase, omega = control.bode(Gs, omega=omega)
    mag = 20 * np.log10(mag)  # mag in db
    phase = np.degrees(phase)  # phase in degrees

    # convert numpy arrays to lists
    omega = list(omega)
    phase = list(phase)
    mag = list(mag)

    # round lists to 6 decimal floating digits
    ndigits = 6
    omega = [round(num, ndigits) for num in omega]
    phase = [round(num, ndigits) for num in phase]
    mag = [round(num, ndigits) for num in mag]

    return omega, mag, phase
Ejemplo n.º 12
0
def bode(name, sys, omega, margins=False):
    mag, phase, omega = control.bode(sys, omega, Plot=False)
    mag_dB = 20 * np.log10(mag)
    if margins:
        gm, pm, sm, wg, wp, ws = control.stability_margins(sys)

    plt.subplot(211)
    if margins:
        plt.hlines(0, omega[0], omega[-1], linestyle='--')
        plt.vlines([wp, wg], np.min(mag_dB), np.max(mag_dB), linestyle='--')
    plt.semilogx(omega, mag_dB)
    plt.xlabel('rad')
    plt.ylabel('dB')
    plt.grid()

    if margins:
        plt.title(
            name +
            ' bode pm: {:0.2f} deg @{:0.2f} rad/s gm: {:0.2f} @{:0.2f} rad/s'.
            format(pm, wp, gm, wg))
    else:
        plt.title(name + ' bode')
    plt.subplot(212)
    phase_deg = np.rad2deg(phase)
    plt.semilogx(omega, phase_deg)
    if margins:
        plt.vlines([wp, wg],
                   np.min(phase_deg),
                   np.max(phase_deg),
                   linestyle='--')
        plt.hlines(-180, omega[0], omega[-1], linestyle='--')

    plt.grid()
Ejemplo n.º 13
0
    def bode(self):
        """
        Display bode plot of the system.

        uses the bode module of control.
        Plots **magnitude in dB** and **phase in degrees** with respect to the
        **Frequency in rad/s**.
        """
        mag, phase, omega = control.bode(self.sys, dB=True)
        plt.clf()
        plt.figure(1)
        plt.subplot(2, 1, 1)
        plt.title("Magnitude Response")
        plt.grid()
        plt.plot(omega, mag, color='g')
        plt.ylabel('Magnitude in dB')
        plt.xlabel('Frequency in rad/s')
        plt.subplot(2, 1, 2)
        plt.title("Phase Response")
        plt.plot(omega, phase, color='r')
        plt.ylabel('Phase in degrees')
        plt.xlabel('Frequency in rad/s')
        plt.grid()
        plt.tight_layout()
        plt.show()
Ejemplo n.º 14
0
    def LoadModelTriggered(self, q):

        options = QFileDialog.Options()
        filename, _ = QFileDialog.getOpenFileName(
            self,
            "Select linearized model file:",
            "U:/Loads/i115_gl2012_2B12_IC/Linearization/LinMod/",
            "Matlab Files (*.mat)",
            options=options)
        azimuth_index = 0
        wtg = WTG(filename, azimuth_index)
        kvect = np.linspace(0, 2e10, 1000)

        start = time.time()
        rlist, klist = ctrl.root_locus(wtg.T_G_1, kvect, Plot=False)
        omega_vect = np.logspace(-2, 2, 1000)
        mag, phase, omega = ctrl.bode(wtg.T_G_1, omega_vect, Plot=False)
        magdb = 20 * np.log10(mag)
        phasedeg = phase * 180 / np.pi
        omegahz = omega / 2 / np.pi
        self.frame_plot_1_2.plot_bode(phase, magdb, omegahz)
        self.frame_plot_2_1.plot_bode(phase, magdb, omegahz)
        self.frame_plot_2_2.plot_bode(phase, magdb, omegahz)

        for ii in range(0, rlist.shape[1]):
            locs = rlist[:, ii]
            x = locs.real
            y = locs.imag
            self.frame_plot_1_1.plot_rlocus.plot(
                x, y, pen=pg.mkPen('k', width=1, style=QtCore.Qt.SolidLine))

        plt.show()
        end = time.time()
        print(end - start)
        print('done')
    def stability_analysis(self):
        """ Perform a stability analysis of the loop with 
            settings defined in the constructor """

        # create the phase detector transfer function
        K_pd = control.tf([1], [1])

        # create the low pass filter transfer function
        iir_taps = self.iir.read_coeffs()
        iir_b = iir_taps[0:2]
        iir_a = iir_taps[3:5]
        K_lpf = control.tf(iir_b, iir_a)

        # create the PI filter transfer function
        K_pi = control.tf([self.proportional_gain, self.integral_gain], [1, 0])

        # create the VCO transfer function
        K_vco = control.tf([1], [1, 0])

        # create the final transfer function
        K_ol = K_pd * K_lpf * K_pi * K_vco

        # find the bode plot
        mag, phase, omega = control.bode(K_ol)

        # plot the bode plot
        plt.plot(omega / (2 * np.pi), mag)
        plt.plot(omega / (2 * np.pi), phase)
        plt.show()

        # find the gain and phase margins
        gm, pm, wg, wp = control.margin(K_ol)
        print("Gain margin = ", gm, "Phase margin = ", pm)

        print(K_ol)
Ejemplo n.º 16
0
def plot_margins(sys):
    mag, phase, omega = ctl.bode(sys, dB=True, Plot=False)
    magdB = 20 * np.log10(mag)
    phase_deg = phase * 180.0 / np.pi
    Gm, Pm, Wcg, Wcp = ctl.margin(sys)
    GmdB = 20 * np.log10(Gm)
    ##Plot Gain and Phase
    f, (ax1, ax2) = plt.subplots(2, 1)
    ax1.semilogx(omega, magdB)
    ax1.grid(which="both")
    ax1.set_xlabel('Frequency (rad/s)')
    ax1.set_ylabel('Magnitude (dB)')
    ax2.semilogx(omega, phase_deg)
    ax2.grid(which="both")
    ax2.set_xlabel('Frequency (rad/s)')
    ax2.set_ylabel('Phase (deg)')
    ax1.set_title('Gm = ' + str(np.round(GmdB, 2)) + ' dB (at ' +
                  str(np.round(Wcg, 2)) + ' rad/s), Pm = ' +
                  str(np.round(Pm, 2)) + ' deg (at ' + str(np.round(Wcp, 2)) +
                  ' rad/s)')
    ###Plot the zero dB line
    ax1.plot(omega, 0 * omega, 'k--', lineWidth=2)
    ###Plot the -180 deg lin
    ax2.plot(omega, -180 + 0 * omega, 'k--', lineWidth=2)
    ##Plot the vertical line from -180 to 0 at Wcg
    ax2.plot([Wcg, Wcg], [-180, 0], 'r--', lineWidth=2)
    ##Plot the vertical line from -180+Pm to 0 at Wcp
    ax2.plot([Wcp, Wcp], [-180, -180 + Pm], 'g--', lineWidth=2)
    ##Plot the vertical line from min(magdB) to 0-GmdB at Wcg
    ax1.plot([Wcg, Wcg], [-GmdB, 0], 'r--', lineWidth=2)
    ##Plot the vertical line from min(magdB) to 0db at Wcp
    ax1.plot([Wcp, Wcp], [np.min(magdB), 0], 'g--', lineWidth=2)
    return Gm, Pm, Wcg, Wcp
Ejemplo n.º 17
0
 def bode_open(self,
               omega: list = None) -> Tuple[np.float, np.float, np.float]:
     mag, phase, omega = ct.bode(self.open_system(),
                                 omega=omega,
                                 dB=True,
                                 deg=True,
                                 Plot=False)
     return mag, phase, omega
Ejemplo n.º 18
0
    def P_analyze(self, system, controller):
        self.S = system
        self.C = controller

        self.C.sysOpenCE(self.S.CE)
        print("Open Loop: %s" % self.C.sysOCE)
        self.C.sysClosedCE()
        print("Closed Loop: %s" % self.C.sysCCE)

        # plot open loop Bode
        self.C.sysRecomp(P=100)
        OC1 = self.C.sysOpenCE(self.S.CE)
        self.C.sysRecomp(P=500)
        OC2 = self.C.sysOpenCE(self.S.CE)
        self.C.sysRecomp(P=2000)
        OC3 = self.C.sysOpenCE(self.S.CE)
        control.bode([OC1, OC2, OC3])
Ejemplo n.º 19
0
 def bode_close(self, omega: list = None) -> Tuple[list, list, list]:
     mag, phase, omega_returned = ct.bode(self.full_system(),
                                          omega=omega,
                                          dB=True,
                                          Plot=False)  # dB=True, deg=True,
     phase = phase * 180. / np.pi
     mag = 20. * np.log10(mag)
     return mag, phase, omega_returned
Ejemplo n.º 20
0
def mybode(*args, **kwargs):
    freq = kwargs.pop('freq', None)
    kwargs['omega'] = freq * 2.0 * np.pi
    mag, phase, omega = bode(*args, **kwargs)
    phase = degwrap(phase)
    mag = FrequencySeries(mag, frequencies=freq)
    phase = FrequencySeries(phase, frequencies=freq)
    return mag, phase
Ejemplo n.º 21
0
 def test_discrete_bode(self, tsys):
     # Create a simple discrete time system and check the calculation
     sys = TransferFunction([1], [1, 0.5], 1)
     omega = [1, 2, 3]
     mag_out, phase_out, omega_out = bode(sys, omega)
     H_z = list(map(lambda w: 1./(np.exp(1.j * w) + 0.5), omega))
     np.testing.assert_array_almost_equal(omega, omega_out)
     np.testing.assert_array_almost_equal(mag_out, np.absolute(H_z))
     np.testing.assert_array_almost_equal(phase_out, np.angle(H_z))
Ejemplo n.º 22
0
def test_initial_phase(TF, initial_phase, default_phase, expected_phase):
    # Check initial phase of standard transfer functions
    mag, phase, omega = ctrl.bode(TF)
    assert (abs(phase[0] - default_phase) < 0.1)

    # Now reset the initial phase to +180 and see if things work
    mag, phase, omega = ctrl.bode(TF, initial_phase=initial_phase)
    assert (abs(phase[0] - expected_phase) < 0.1)

    # Make sure everything works in rad/sec as well
    if initial_phase:
        plt.xscale('linear')  # avoids xlim warning on next line
        plt.clf()  # clear previous figure (speeds things up)
        mag, phase, omega = ctrl.bode(TF,
                                      initial_phase=initial_phase / 180. *
                                      math.pi,
                                      deg=False)
        assert (abs(phase[0] - expected_phase) < 0.1)
Ejemplo n.º 23
0
def add_spec_input_disturbance(gamma_d, omega_d, system, dB_flag=False):
    fig = plt.gcf()
    w = np.logspace(np.log10(omega_d)-2, np.log10(omega_d))
    mag, phase, omega = bode(system, dB=dB_flag, omega=w, Plot=False)
    if dB_flag == False:
        fig.axes[0].loglog(w, 1. / gamma_d * np.ones(len(w))*mag,
                 '--', color=[1, 0, 0], label='$d_{in}$ spec')
    else:
        fig.axes[0].semilogx(w, 20 * np.log10(1 / gamma_r) * ones(size(w)),
                 '--', color=[1, 0, 0], label='$d_{in}$ spec')
 def urCheck(sys,wctar,pmtar,omega):
     thres=0.5
     R.urOffset = 1
     gm, pm, wg, wp = ctrl.margin(sys)
     mag, phaserad, w = ctrl.bode(sys, omega)
     phasedeg = phaserad * 180 / pi
     if pm < pmtar:
         for i in range(len(omega)):
             if phasedeg[i] < pmtar-180+thres and phasedeg[i] > pmtar-180-thres and mag[i]>1:
                 R.urOffset=mag[i]**-1
                 break
     return sys * R.urOffset
def speed():
    vmax = 1
    kl = 3.1
    lmax = (vmax / kl)**2
    lrange = np.arange(-lmax, lmax, 0.0001)[np.newaxis].T
    vrange = (np.sign(lrange) * kl * (np.abs(lrange))**0.5)
    k1 = np.linalg.pinv(lrange) @ vrange
    print(k1)
    #plt.plot(lrange,vrange)
    #plt.plot(lrange,k1*lrange)
    #plt.show()

    #plt.clf()

    Hs = ct.tf([kl], [1])
    Hz = ct.matlab.c2d(Hs, 1, method='zoh')

    Fs = 1
    Tp = 100
    Tz = 1000
    p1 = np.exp(-1 / Tp / Fs)
    z1 = np.exp(-1 / Tz / Fs)
    p = [-.5, p1]  #pole locations
    z = [-.5, z1]  #zero loations
    gain = 0.002  #gain
    freq = 0.1  #at frequency
    Fs = 1  #sample rate
    #fn.zplane(p,z)
    #plt.show()
    k = gain / np.abs((1 - z[0] * np.exp(-freq / Fs * 1j)) *
                      (1 - z[1] * np.exp(-freq / Fs * 1j)) /
                      ((1 - p[0] * np.exp(-freq / Fs * 1j)) *
                       (1 - p[1] * np.exp(-freq / Fs * 1j))))
    b = [k, -k * (z[0] + z[1]), k * z[0] * z[1]]
    a = [1, -(p[0] + p[1]), p[0] * p[1]]
    #print(a,b)
    Kz = ct.tf(b, a, 1 / Fs)

    ct.bode(Kz * Hz, np.logspace(-5, 0))
    plt.show()
 def updateClosedLoop(self):
     if not self.is_system_identified:
         return
     # Simulate closed-loop system with generated PID
     num = self.num
     den = self.den
     dt = self.dt
     kc = self.kc
     ki = self.ki
     kd = self.kd
     Gz2 = ctrl.TransferFunction(num, den, dt)
     (pid_num, pid_den) = gainsToNumDen(kc, ki, kd, dt)
     PID = ctrl.TransferFunction(pid_num, pid_den, dt)
     Gcl = ctrl.feedback(PID * Gz2, 1)
     t_out, y_out = ctrl.step_response(Gcl, T=np.arange(0, 1, dt))
     self.plotClosedLoop(t_out, y_out)
     w = np.logspace(-1, 3, 40).tolist()
     mag, phase, omega = ctrl.bode(Gz2, omega=np.asarray(w), plot=False)
     mag_cl, phase_cl, omega_cl = ctrl.bode(Gcl,
                                            omega=np.asarray(w),
                                            plot=False)
     self.plotBode(omega, mag, omega_cl, mag_cl)
Ejemplo n.º 27
0
def frequency_requirements(g, gain_margin=None, phase_margin=None):
    gains = []

    gm, pm, _, _ = ct.margin(g)
    if gain_margin is not None:
        gains.append(10**(-(gain_margin - gm) / 20))

    if phase_margin is not None:
        mag, phase, omega = ct.bode(g, Plot=False)
        arg = np.argmin(abs(phase - (phase_margin - np.pi)))
        m = 20 * np.log10(mag[arg])
        gains.append(10**(-m / 20))

    return np.prod(gains)
Ejemplo n.º 28
0
def lag_compensator(g,
                    err_step=None,
                    err_ramp=None,
                    err_para=None,
                    pm_desired=None,
                    psi=None):
    s = ct.TransferFunction([1, 0], [1])
    if psi is not None:
        pm_desired = 100 * psi * np.pi / 180

    if pm_desired > 2 * np.pi:
        print("remember, I need phase in radians")

    kc, ess = ss_error(g, err_step, err_ramp, err_para)

    if abs(ess) == np.inf or abs(ess) == np.nan:
        ess = None

    if ess is None or kc is None:
        assert "Inconsistent system"

    kc = kc / np.real(ess)

    print("kc=", kc)

    phi = (pm_desired + 7 * np.pi / 180) - np.pi

    print("phi=", phi)

    mag, phase, omega = ct.bode(kc * g, Plot=False)
    arg = np.argmin(abs(phase - phi))

    wcg = omega[arg]
    print(f"wcg= {wcg}")

    gain = 20 * np.log10(mag[arg])
    a = 0 - gain
    print(f"A= {a}")

    alpha = 10**(a / 20)
    tau = 10 / alpha / wcg

    lag = kc * (alpha * tau * s + 1) / (tau * s + 1)

    return lag
Ejemplo n.º 29
0
def lead_compensator(g,
                     err_step=None,
                     err_ramp=None,
                     err_para=None,
                     pm_desired=None,
                     psi=None):
    s = ct.TransferFunction([1, 0], [1])
    # all radians policy
    if psi is not None:
        pm_desired = 100 * psi * np.pi / 180

    if pm_desired > 2 * np.pi:
        print("remember, I need phase in radians")
    kc, ess = ss_error(g, err_step, err_ramp, err_para)

    if abs(ess) == np.inf or abs(ess) == np.nan:
        ess = None

    if ess is None or kc is None:
        assert "Inconsistent system"

    kc = kc / np.real(ess)
    print(f"kc= {kc}")

    _, pm, _, wpm = ct.margin(kc * g)
    print(pm, wpm)
    k_angle = (pm_desired - pm * np.pi / 180 + 5 * np.pi / 180)  # 5deg extra

    alpha = (1 + np.sin(k_angle)) / (1 - np.sin(k_angle))
    print(f"alpha= {alpha}")
    a = 10 * np.log10(alpha)

    print(f"A= {a}")

    mag, phase, omega = ct.bode(kc * g, Plot=False)
    arg = np.argmin(abs(20 * np.log10(mag) - -a))
    wcg = omega[arg]

    tau = 1 / np.sqrt(alpha) / wcg

    print(f"tau= {tau}")

    lead = kc * (alpha * tau * s + 1) / (tau * s + 1)
    return lead
Ejemplo n.º 30
0
def bode(tf: Union[control.TransferFunction, control.StateSpace],
         deg: bool = True,
         dB: bool = True,
         Hz: bool = True,
         Plot: bool = True):
    """Computer bode plot, to fix issues with bode from python control"""
    mag, phase, w = control.bode(tf, deg=deg, dB=dB, Hz=Hz, Plot=False)
    if Plot:
        plt.subplot(211)
        plt.semilogx(w, mag)
        plt.grid(which='both')
        plt.ylabel('magnitude (dB)')

        plt.subplot(212)
        plt.semilogx(w, phase)
        plt.xlabel('Hz')
        plt.ylabel('phase (deg)')
        plt.grid(which='both')
    return mag, phase, w
Ejemplo n.º 31
0
    def testConvert(self):
        """Test state space to transfer function conversion."""
        verbose = self.debug
        from control.statesp import _mimo2siso
        
        #print __doc__

        # Machine precision for floats.
        eps = np.finfo(float).eps

        for states in range(1, self.maxStates):
            for inputs in range(1, self.maxIO):
                for outputs in range(1, self.maxIO):
                    # start with a random SS system and transform to TF then
                    # back to SS, check that the matrices are the same.
                    ssOriginal = matlab.rss(states, outputs, inputs)
                    if (verbose):
                        self.printSys(ssOriginal, 1)

                    # Make sure the system is not degenerate
                    Cmat = control.ctrb(ssOriginal.A, ssOriginal.B)
                    if (np.linalg.matrix_rank(Cmat) != states):
                        if (verbose):
                            print("  skipping (not reachable)")
                        continue
                    Omat = control.obsv(ssOriginal.A, ssOriginal.C)
                    if (np.linalg.matrix_rank(Omat) != states):
                        if (verbose):
                            print("  skipping (not observable)")
                        continue

                    tfOriginal = matlab.tf(ssOriginal)
                    if (verbose):
                        self.printSys(tfOriginal, 2)
                    
                    ssTransformed = matlab.ss(tfOriginal)
                    if (verbose):
                        self.printSys(ssTransformed, 3)

                    tfTransformed = matlab.tf(ssTransformed)
                    if (verbose):
                        self.printSys(tfTransformed, 4)

                    # Check to see if the state space systems have same dim
                    if (ssOriginal.states != ssTransformed.states):
                        print("WARNING: state space dimension mismatch: " + \
                            "%d versus %d" % \
                            (ssOriginal.states, ssTransformed.states))

                    # Now make sure the frequency responses match
                    # Since bode() only handles SISO, go through each I/O pair
                    # For phase, take sine and cosine to avoid +/- 360 offset
                    for inputNum in range(inputs):
                        for outputNum in range(outputs):
                            if (verbose):
                                print("Checking input %d, output %d" \
                                    % (inputNum, outputNum))
                            ssorig_mag, ssorig_phase, ssorig_omega = \
                                control.bode(_mimo2siso(ssOriginal, \
                                                        inputNum, outputNum), \
                                                 deg=False, Plot=False)
                            ssorig_real = ssorig_mag * np.cos(ssorig_phase)
                            ssorig_imag = ssorig_mag * np.sin(ssorig_phase)

                            #
                            # Make sure TF has same frequency response
                            #
                            num = tfOriginal.num[outputNum][inputNum]
                            den = tfOriginal.den[outputNum][inputNum]
                            tforig = control.tf(num, den)
                                                
                            tforig_mag, tforig_phase, tforig_omega = \
                                control.bode(tforig, ssorig_omega, \
                                                 deg=False, Plot=False)

                            tforig_real = tforig_mag * np.cos(tforig_phase)
                            tforig_imag = tforig_mag * np.sin(tforig_phase)
                            np.testing.assert_array_almost_equal( \
                                ssorig_real, tforig_real)
                            np.testing.assert_array_almost_equal( \
                                ssorig_imag, tforig_imag)

                            #
                            # Make sure xform'd SS has same frequency response
                            #
                            ssxfrm_mag, ssxfrm_phase, ssxfrm_omega = \
                                control.bode(_mimo2siso(ssTransformed, \
                                                        inputNum, outputNum), \
                                                 ssorig_omega, \
                                                 deg=False, Plot=False)
                            ssxfrm_real = ssxfrm_mag * np.cos(ssxfrm_phase)
                            ssxfrm_imag = ssxfrm_mag * np.sin(ssxfrm_phase)
                            np.testing.assert_array_almost_equal( \
                                ssorig_real, ssxfrm_real)
                            np.testing.assert_array_almost_equal( \
                                ssorig_imag, ssxfrm_imag)

                            #
                            # Make sure xform'd TF has same frequency response
                            #
                            num = tfTransformed.num[outputNum][inputNum]
                            den = tfTransformed.den[outputNum][inputNum]
                            tfxfrm = control.tf(num, den)
                            tfxfrm_mag, tfxfrm_phase, tfxfrm_omega = \
                                control.bode(tfxfrm, ssorig_omega, \
                                                 deg=False, Plot=False)
                            
                            tfxfrm_real = tfxfrm_mag * np.cos(tfxfrm_phase)
                            tfxfrm_imag = tfxfrm_mag * np.sin(tfxfrm_phase)
                            np.testing.assert_array_almost_equal( \
                                ssorig_real, tfxfrm_real)
                            np.testing.assert_array_almost_equal( \
                                ssorig_imag, tfxfrm_imag)
import control
from matplotlib import pyplot as plt
from utils import feedback

w = np.logspace(-1, 2, 1000)
s = control.tf([1, 0], 1)
G = 4 /((s - 1)*(0.02*s + 1)**2)
Kc = 1.25
tau1 = 1.5
K = Kc*(1+1/(tau1*s))
L = K*G
S = feedback(1, L)
T = feedback(L, 1)


mag, phase, omega = control.bode(L, w)
magS, phaseS, omega = control.bode(S, w)
magT, phaseT, omega = control.bode(T, w)
plt.legend(["L", "S", "T"],
           bbox_to_anchor=(0, 1.01, 1, 0), loc=3, ncol=3)

Ms = max(magS)
Mt = max(magT)

gm, pm, wg, wp = control.margin(mag, phase, omega)

Lu_180 = 1/np.abs(control.evalfr(L, wg))
P = np.angle(control.evalfr(L, wp)) + np.pi

print "Lower GM:", Lu_180
print "PM:", np.round(P*180/np.pi, 1), "deg or", np.round(P, 2), "rad"
Ejemplo n.º 33
0
from matplotlib.pyplot import *
from scipy import *

import control
import bode_utils

G = control.TransferFunction(100.0,[1,0,0])#100.0/s**2

f = logspace(-2,2,1000)
w = 2*pi*f

db, phase, f2 = control.bode(G, omega=w, dB=True, Hz=True, Plot=False)

bode_utils.bode_plot(f, db, phase, clear=True, fignum=1, label='$G$')

#Choose freq and design a lead comp.
f_c = 10.0
w_c = 2*pi*f_c
factor = 5.0
z = w_c/factor
p = w_c*factor
G_lead = control.TransferFunction([1,z],[1,p])*p/z

gain = 6.0

db2, phase2, f2 = control.bode(G*G_lead*gain, omega=w, dB=True, Hz=True, Plot=False)

bode_utils.bode_plot(f, db2, phase2, clear=False, fignum=1, label='$G_c \\cdot G$')

subplot(211)
legend(loc=3)
Ejemplo n.º 34
0
#!/usr/bin/env python

from matplotlib.pyplot import plot, legend, savefig, gcf
from numpy import logspace
from control import bode, tf

T=1

G = tf([0, 1], [T, 1])
om = logspace(-2, 2, 100)

bode(G, dB=True, omega=om)

f=gcf()
r, mag, fas = f.get_children()
p1, = mag.plot([0, 0.1, 1, 10, 100], [0, 0, 0, -20, -40], '--')
fas.plot([0, 0.1, 1, 10, 100], [0, 0, -45, -90, -90], '--')

mag.set_yticks([0, -3, -10, -20, -40])

fas.set_yticks([0, -45, -90])

mag.legend([p1], ['asintotas'])
mag.set_xticklabels([])
fas.set_xticklabels(['',r'$\frac{1}{100 T}$', r'$\frac{1}{10 T}$', r'$\frac{1}{T}$', r'$\frac{10}{T}$', r'$\frac{100}{T}$'])
#a.set_yticklabels([])

# Se guarda la figura en la misma carpeta
savefig("bodeprimerorden.pdf", bbox_inches='tight', pad_inches=0, transparent="True")
Ejemplo n.º 35
-5
def plot_loops(name, G_ol, G_cl):
    """
    Plot loops
    :param name: Name of axis
    :param G_ol: open loop transfer function
    :param G_cl: closed loop transfer function
    """
    plt.figure()
    plt.plot(*control.step_response(G_cl, np.linspace(0, 1, 1000)))
    plt.title(name + ' step resposne')
    plt.grid()

    plt.figure()
    control.bode(G_ol)
    print('margins', control.margin(G_ol))
    plt.subplot(211)
    plt.title(name + ' open loop bode plot')

    plt.figure()
    control.rlocus(G_ol, np.logspace(-2, 0, 1000))
    for pole in G_cl.pole():
        plt.plot(np.real(pole), np.imag(pole), 'rs')
    plt.title(name + ' root locus')
    plt.grid()