예제 #1
0
def classic():
    # PI controller
    Kp = 1.5
    Ki = 30.
    
    P = tf([Kp], [1])
    I = tf([Ki], [1, 0])
    
    Gc = parallel(P, I)
    
    # Power convertor
    Gpc = synthesize_2pole_filter(50, 0.707)
    
    # Plant
    Gp = tf([50], [1, 0])
    
    # Sensor
    # если выбросить из петли становится лучше
    # "remove sensor lag"
    Gs = synthesize_1pole_filter(20)
    
    # Openloop
    Gol = series(series(Gc, Gpc), Gp)
    
    # Closed loop
    Gcl = feedback(Gol)
    
    ts, xs = step_response( Gcl )
    
    grid()
    plot(ts, xs)
예제 #2
0
    def testLists(self):
        """Make sure that lists of various lengths work for operations"""
        sys1 = ctrl.tf([1, 1], [1, 2])
        sys2 = ctrl.tf([1, 3], [1, 4])
        sys3 = ctrl.tf([1, 5], [1, 6])
        sys4 = ctrl.tf([1, 7], [1, 8])
        sys5 = ctrl.tf([1, 9], [1, 0])

        # Series
        sys1_2 = ctrl.series(sys1, sys2)
        np.testing.assert_array_almost_equal(sort(pole(sys1_2)), [-4., -2.])
        np.testing.assert_array_almost_equal(sort(zero(sys1_2)), [-3., -1.])
        
        sys1_3 = ctrl.series(sys1, sys2, sys3);
        np.testing.assert_array_almost_equal(sort(pole(sys1_3)),
                                             [-6., -4., -2.])
        np.testing.assert_array_almost_equal(sort(zero(sys1_3)), 
                                             [-5., -3., -1.])
        
        sys1_4 = ctrl.series(sys1, sys2, sys3, sys4);
        np.testing.assert_array_almost_equal(sort(pole(sys1_4)),
                                             [-8., -6., -4., -2.])
        np.testing.assert_array_almost_equal(sort(zero(sys1_4)),
                                             [-7., -5., -3., -1.])
        
        sys1_5 = ctrl.series(sys1, sys2, sys3, sys4, sys5);
        np.testing.assert_array_almost_equal(sort(pole(sys1_5)),
                                             [-8., -6., -4., -2., -0.])
        np.testing.assert_array_almost_equal(sort(zero(sys1_5)), 
                                             [-9., -7., -5., -3., -1.])

        # Parallel 
        sys1_2 = ctrl.parallel(sys1, sys2)
        np.testing.assert_array_almost_equal(sort(pole(sys1_2)), [-4., -2.])
        np.testing.assert_array_almost_equal(sort(zero(sys1_2)),
                                             sort(zero(sys1 + sys2)))
        
        sys1_3 = ctrl.parallel(sys1, sys2, sys3);
        np.testing.assert_array_almost_equal(sort(pole(sys1_3)),
                                             [-6., -4., -2.])
        np.testing.assert_array_almost_equal(sort(zero(sys1_3)), 
                                             sort(zero(sys1 + sys2 + sys3)))
        
        sys1_4 = ctrl.parallel(sys1, sys2, sys3, sys4);
        np.testing.assert_array_almost_equal(sort(pole(sys1_4)),
                                             [-8., -6., -4., -2.])
        np.testing.assert_array_almost_equal(sort(zero(sys1_4)), 
                                             sort(zero(sys1 + sys2 + 
                                                       sys3 + sys4)))

        
        sys1_5 = ctrl.parallel(sys1, sys2, sys3, sys4, sys5);
        np.testing.assert_array_almost_equal(sort(pole(sys1_5)),
                                             [-8., -6., -4., -2., -0.])
        np.testing.assert_array_almost_equal(sort(zero(sys1_5)), 
                                             sort(zero(sys1 + sys2 + 
                                                       sys3 + sys4 + sys5)))
예제 #3
0
    def testLists(self):
        """Make sure that lists of various lengths work for operations"""
        sys1 = ctrl.tf([1, 1], [1, 2])
        sys2 = ctrl.tf([1, 3], [1, 4])
        sys3 = ctrl.tf([1, 5], [1, 6])
        sys4 = ctrl.tf([1, 7], [1, 8])
        sys5 = ctrl.tf([1, 9], [1, 0])

        # Series
        sys1_2 = ctrl.series(sys1, sys2)
        np.testing.assert_array_almost_equal(sort(pole(sys1_2)), [-4., -2.])
        np.testing.assert_array_almost_equal(sort(zero(sys1_2)), [-3., -1.])
        
        sys1_3 = ctrl.series(sys1, sys2, sys3);
        np.testing.assert_array_almost_equal(sort(pole(sys1_3)),
                                             [-6., -4., -2.])
        np.testing.assert_array_almost_equal(sort(zero(sys1_3)), 
                                             [-5., -3., -1.])
        
        sys1_4 = ctrl.series(sys1, sys2, sys3, sys4);
        np.testing.assert_array_almost_equal(sort(pole(sys1_4)),
                                             [-8., -6., -4., -2.])
        np.testing.assert_array_almost_equal(sort(zero(sys1_4)),
                                             [-7., -5., -3., -1.])
        
        sys1_5 = ctrl.series(sys1, sys2, sys3, sys4, sys5);
        np.testing.assert_array_almost_equal(sort(pole(sys1_5)),
                                             [-8., -6., -4., -2., -0.])
        np.testing.assert_array_almost_equal(sort(zero(sys1_5)), 
                                             [-9., -7., -5., -3., -1.])

        # Parallel 
        sys1_2 = ctrl.parallel(sys1, sys2)
        np.testing.assert_array_almost_equal(sort(pole(sys1_2)), [-4., -2.])
        np.testing.assert_array_almost_equal(sort(zero(sys1_2)),
                                             sort(zero(sys1 + sys2)))
        
        sys1_3 = ctrl.parallel(sys1, sys2, sys3);
        np.testing.assert_array_almost_equal(sort(pole(sys1_3)),
                                             [-6., -4., -2.])
        np.testing.assert_array_almost_equal(sort(zero(sys1_3)), 
                                             sort(zero(sys1 + sys2 + sys3)))
        
        sys1_4 = ctrl.parallel(sys1, sys2, sys3, sys4);
        np.testing.assert_array_almost_equal(sort(pole(sys1_4)),
                                             [-8., -6., -4., -2.])
        np.testing.assert_array_almost_equal(sort(zero(sys1_4)), 
                                             sort(zero(sys1 + sys2 + 
                                                       sys3 + sys4)))

        
        sys1_5 = ctrl.parallel(sys1, sys2, sys3, sys4, sys5);
        np.testing.assert_array_almost_equal(sort(pole(sys1_5)),
                                             [-8., -6., -4., -2., -0.])
        np.testing.assert_array_almost_equal(sort(zero(sys1_5)), 
                                             sort(zero(sys1 + sys2 + 
                                                       sys3 + sys4 + sys5)))
예제 #4
0
def multi_loop():
    """
        Gives the transfer funtion of multi-loop system

        Feedback[ R(s) -> (G1 - H2) -> G2 -> feedback(G3G4, H1) , H3]-> C(s)
    """
    g1 = np.array([
        [1], [1, 10]])

    g2 = np.array([
        [1],
        [1, 1]]
    )
    g3 = np.array([
        [1, 0, 1],
        [1, 4, 4]
    ])
    g4 = np.array([
        [1, 1],
        [1, 6]
    ])

    h1 = np.array([
        [1, 1],
        [1, 2]
    ])
    h2 = np.array([[2], 1])
    h3 = np.array([[1], 1])

    sys_g1 = get_sys(g1, sys_name='tf')
    sys_g2 = get_sys(g2, 'tf')
    sys_g3 = get_sys(g3, 'tf')
    sys_g4 = get_sys(g4, 'tf')
    sysh1 = get_sys(h1, 'tf')
    sysh2 = get_sys(h2, 'tf')
    sysh3 = get_sys(h3, 'tf')

    td.ouput(title='Exercise One',
             keys=['G1', 'G2', 'G3', 'G4', 'H1', 'H2', 'H3'],
             values=[sys_g1, sys_g2, sys_g3, sys_g4, sysh1, sysh2, sysh3])
    sys_t = ct.series(sys_g1 - sysh2, sys_g2)
    sys_t = ct.series(sys_t,
                      ct.feedback(
                          ct.series(sys_g3, sys_g4), sysh1, sign=1))
    tS = ct.feedback(sys_t, sysh3)
    zeros, poles = ct.pzmap(tS, Plot=True, grid=True)

    show_plot(zeros, poles)
예제 #5
0
def delay(sys, T):
    """========================================================================
    Function: delay() 
        approximates the given delay with a PT100 element and concatenates it 
        with the given system.
    
    Synopsis:
        sys1 = delay(sys,T)
    
    Input Parameter:
        - sys: Control module LTI-System in transfer function or state space
            formulation. The delay will be added to this system.
        - T: Delay time that will be added to this system.
    
    Output Parameter:
        - sys1: The delayed input system.
    ========================================================================"""
    # define the PT100 element
    n = 100
    T = float(T) / n
    A = (np.diag([-1] * n, 0) + np.diag([1] * (n - 1), -1)) / T
    B = np.vstack([1, [[0]] * (n - 1)]) / T
    C = np.hstack([[0] * (n - 1), 1])
    D = 0
    # concatenate it with the system
    return ctrl.series(sys, ctrl.ss(A, B, C, D))
예제 #6
0
def serify(called=False):
    """
        Generates a system of transfer functions for
        a series block combination
    """

    # Create arrays for G(s)
    numerator_g = np.array([1])
    denominator_g = np.array([500, 0, 0])

    # Create arrays for G(c)
    numerator_h = np.array([1, 1])
    denominator_h = np.array([1, 2])

    # Get transfer functions
    sys_g = ct.tf(numerator_g, denominator_g)
    sys_h = ct.tf(numerator_h, denominator_h)
    sys_tranfer_func = ct.series(sys_g, sys_h)

    if called:
        return sys_g, sys_h

    td.ouput('Series TF',
             keys=['G(s)', 'G(c)', 'sys tf'],
             values=[sys_g, sys_h, sys_tranfer_func])
예제 #7
0
    def calculating_params(self, populations):
        calculatedParams = []
        for population in populations:
            kp = population[0]
            ti = population[1]
            td = population[2]
            G = kp * control.tf([ti * td, ti, 1], [ti, 0])
            F = control.tf(1, [1, 6, 11, 6, 0])
            system = control.feedback(control.series(G, F), 1)

            t = []
            i = 0
            while i < 100:
                t.append(i)
                i += 0.01

            try:
                systemInfo = control.step_info(system)
            except IndexError:
                calculatedParams.append([10000000, 1, 100, 200])
                continue

            T, output = control.step_response(system, T=t)

            ISE = round(sum((output - 1)**2), 2)
            timeRise = round(systemInfo['RiseTime'], 2)
            timeSettle = round(systemInfo['SettlingTime'], 2)
            overshoot = round(systemInfo['Overshoot'], 2)
            calculatedParams.append([ISE, timeRise, timeSettle, overshoot])

        return calculatedParams
예제 #8
0
def pidplot1(num, den, kp, ki, kd):
    G = control.tf(num, den)
    if ki != 0:
        g2 = control.tf([kd, kp, ki], [1, 0])
    else:
        g2 = control.tf([kd, kp], [1])
    k = control.series(G, g2)
    v = control.feedback(k, 1)
    t1, y1 = control.step_response(G)
    t2, y2 = control.step_response(v)
    plt.plot(t2, y2, 'r', linewidth=1, label='G(s) with PID control')
    plt.grid(True)
    s = '(Kp=' + str(kp) + ',Ki=' + str(ki) + ',Kd=' + str(kd) + ')'
    plt.title('Time response of G(s) with PID control' + s)
    plt.xlabel('Time(sec)')
    plt.ylabel('Amplitude')
    if not os.path.isdir('static'):
        os.mkdir('static')
    else:
        # Remove old plot files
        for filename in glob.glob(
                os.path.join('static', 'pidcontrol', 'pidc1.png')):
            os.remove(filename)

    pidc1 = os.path.join('static', 'pidcontrol', 'pidc1.png')
    plt.savefig(pidc1)
    plt.clf()
    plt.cla()
    plt.close()
    return pidc1
예제 #9
0
def fitnessFunction(Kp, Ti, Td):
    G = control.tf([Ti * Td, Ti, 1], [Ti, 0])

    F = control.tf(1, [1, 6, 11, 6, 0])

    sys = control.feedback(control.series(G, F), 1)

    y = control.step(sys)

    yout = y[0]
    t = y[-1]

    # Integrated Square Error
    error = 0

    for val in y[0]:
        error += (val - 1) * (val - 1)

    # Overshoot
    OS = (yout.max() / yout[-1] - 1) * 100

    Tr = 0
    Ts = 0

    # Rising Time
    for i in range(0, len(yout) - 1):
        if yout[i] > yout[-1] * .90:
            Tr = t[i] - t[0]

    # Settling Time
    for i in range(2, len(yout) - 1):
        if abs(yout[-i] / yout[-1]) > 1.02:
            Ts = t[len(yout) - i] - t[0]

    return 1 / (OS + Tr + Ts + error * error * 10) * 100000
def bode_cor(num1, den1, num2, den2):
    sys1 = ctl.tf(num1, den1)
    sys2 = ctl.tf(num2, den2)
    sys_BO = ctl.series(sys1, sys2)
    sys = sys_BO.returnScipySignalLti()[0][0]
    w, mag, phase = sys.bode()
    return w, mag, phase
예제 #11
0
def fitnessFunction(Kp, Ti, Td):
    G = control.tf([Ti*Td, Ti, 1], [Ti, 0])
    
    F = control.tf(1,[1,6,11,6,0])
    
    sys = control.feedback(control.series(G, F), 1)
    
    y = control.step(sys)
    
    yout = y[0]
    t = y[-1]
    
    # Integrated Square Error
    error = 0
    
    for val in y[0]:
        error += (val - 1)*(val - 1)
       
    # Overshoot
    OS = (yout.max()/yout[-1]-1)*100
    
    Tr = 0
    Ts = 0
    
    # Rising Time
    for i in range(0, len(yout) - 1):
        if yout[i] > yout[-1]*.90:
            Tr = t[i] - t[0]
    
    # Settling Time            
    for i in range(2, len(yout) - 1):
        if abs(yout[-i]/yout[-1]) > 1.02:
            Ts = t[len(yout) - i] - t[0]
    
    return 1/(OS + Tr + Ts + error*error*10)*100000
예제 #12
0
def Controlador(X):

    #Atribuição dos paramentros Kp, Ki e Kd
    Kp = Kpi + X[0] / 100
    Ki = Kii + X[1] / 100
    Kd = Kdi + X[2] / 100

    #Função Transferência dos termos do controlador
    P = Kp
    I = tf(Ki, [1, 0])
    D = tf([Kd, 0], [0.1 * Kd, 1])
    #União dos blocos PID
    C = parallel(P, I, D)

    # Função Transferência com o Controlador
    F = series(C, G)

    # Penalidade para o valor do sinal de Entrada na planta
    # ou seja penalidade para o sinal de Controle alto
    tc = feedback(C, G)
    _, yc = step_response(tc, time)
    if max(yc) > maxcontrolador:
        #SE1 = np.square(np.subtract(1,yc))
        ISE = tdiv + max(yc)
    else:
        # Realizando a Integral do erro quadrado
        t1 = feedback(F, 1)
        _, y1 = step_response(t1, time)
        SE = np.square(np.subtract(1, y1))
        ISE = np.sum(SE)

    return (ISE)
예제 #13
0
def generate_lead_controller(G, testing_point):
    angle_G_evaluated_at_testpoint = math.degrees(
        np.angle(control.evalfr(G, testing_point)))
    defficit = 180 - angle_G_evaluated_at_testpoint
    if defficit < 90:
        new_zero = np.real(testing_point)
        angle_new_pole = 90 - defficit
    else:
        new_zero = 0
        angle_new_zero = math.degrees(
            np.angle(
                control.evalfr(control.series(control.tf([1, 0], 1), G),
                               testing_point)))
        if angle_new_zero < 0:
            angle_new_pole = 180 + angle_new_zero
        else:
            angle_new_pole = -180 - angle_new_zero
    distance_pole_zero = (np.imag(testing_point) /
                          math.tan(math.radians(angle_new_pole)))
    new_pole = np.real(testing_point) - distance_pole_zero
    if new_zero == 0:
        num_and_controler_zero = 1
    num_and_controler_zero = np.concatenate([[1], [-new_zero]])
    den_and_controler_pole = np.concatenate([[1], [-new_pole]])
    lead_controller = control.tf(num_and_controler_zero,
                                 den_and_controler_pole)
    return lead_controller
예제 #14
0
파일: ga.py 프로젝트: tcooc/adaptive4
def Q2_perfFCN(Kp, Ti, Td):
  G = Kp * tf([Ti * Td, Ti, 1.0], [Ti, 0])

  sys = feedback(series(G,F),1)
  res = step_response(sys, t)

  ISE = sum((val - 1)**2 for val in res[1])
  return (ISE,) + stepinfo(res)
예제 #15
0
    def test_bdalg_functions(self):
        """Test block diagram functions algebra on I/O systems"""
        # Set up parameters for simulation
        T = self.T
        U = [np.sin(T), np.cos(T)]
        X0 = 0

        # Set up systems to be composed
        linsys1 = self.mimo_linsys1
        linio1 = ios.LinearIOSystem(linsys1)
        linsys2 = self.mimo_linsys2
        linio2 = ios.LinearIOSystem(linsys2)

        # Series interconnection
        linsys_series = ct.series(linsys1, linsys2)
        iosys_series = ct.series(linio1, linio2)
        lin_t, lin_y, lin_x = ct.forced_response(linsys_series, T, U, X0)
        ios_t, ios_y = ios.input_output_response(iosys_series, T, U, X0)
        np.testing.assert_array_almost_equal(ios_y, lin_y, decimal=3)

        # Make sure that systems don't commute
        linsys_series = ct.series(linsys2, linsys1)
        lin_t, lin_y, lin_x = ct.forced_response(linsys_series, T, U, X0)
        self.assertFalse((np.abs(lin_y - ios_y) < 1e-3).all())

        # Parallel interconnection
        linsys_parallel = ct.parallel(linsys1, linsys2)
        iosys_parallel = ct.parallel(linio1, linio2)
        lin_t, lin_y, lin_x = ct.forced_response(linsys_parallel, T, U, X0)
        ios_t, ios_y = ios.input_output_response(iosys_parallel, T, U, X0)
        np.testing.assert_array_almost_equal(ios_y, lin_y, decimal=3)

        # Negation
        linsys_negate = ct.negate(linsys1)
        iosys_negate = ct.negate(linio1)
        lin_t, lin_y, lin_x = ct.forced_response(linsys_negate, T, U, X0)
        ios_t, ios_y = ios.input_output_response(iosys_negate, T, U, X0)
        np.testing.assert_array_almost_equal(ios_y, lin_y, decimal=3)

        # Feedback interconnection
        linsys_feedback = ct.feedback(linsys1, linsys2)
        iosys_feedback = ct.feedback(linio1, linio2)
        lin_t, lin_y, lin_x = ct.forced_response(linsys_feedback, T, U, X0)
        ios_t, ios_y = ios.input_output_response(iosys_feedback, T, U, X0)
        np.testing.assert_array_almost_equal(ios_y, lin_y, decimal=3)
예제 #16
0
def lag_controller_design(error, percentage_to_reduce, lead_controller, G,
                          K_lead):
    lead_at_0 = control.evalfr(lead_controller, 0)
    plant_at_0 = control.evalfr(G, 0)
    error_lag_lead_ss = error_ss - percentage_to_reduce
    x = Symbol('x')
    equation = (1 / (1 + x * control.evalfr(control.series(lead_controller, G),
                                            0) * K_lead)) - error_lag_lead_ss
    return solve(equation, x), error_lag_lead_ss
예제 #17
0
def compute_ise(Kp, Ti, Td):
    g = Kp * TransferFunction([Ti * Td, Ti, 1], [Ti, 0])
    sys = feedback(series(g, F), 1)
    sys_info = step_info(sys)
    _, y = step_response(sys, T)
    ise = sum((y - 1)**2)
    t_r = sys_info['RiseTime']
    t_s = sys_info['SettlingTime']
    m_p = sys_info['Overshoot']
    return ise, t_r, t_s, m_p
예제 #18
0
def q1_perfFNC(Kp, Ti, Td):
    G = Kp * TransferFunction([Ti * Td, Ti, 1], [Ti, 0])
    sys = feedback(series(G, F), 1)
    sysinf = step_info(sys)

    T, y = step_response(sys, T=t)
    # return ISE, t_r, t_s, M_p
    return sum((
        y -
        1)**2), sysinf['RiseTime'], sysinf['SettlingTime'], sysinf['Overshoot']
예제 #19
0
def pid_controller(K_kr, T_kr):
    num = [10]
    den = [10, 25, 11, 2]
    G = ctl.tf(num, den)
    K_P = 0.6 * K_kr
    K_I = 1.2 * K_kr / T_kr
    K_D = 0.072 * K_kr * T_kr
    K_PID = ctl.parallel(ctl.tf([K_I], [1, 0]), ctl.tf([K_D, 0], [1]), K_P)
    G_0 = ctl.series(K_PID, G)
    return ctl.feedback(G_0)
예제 #20
0
    def testMimoSeries(self, tsys):
        """regression: bdalg.series reverses order of arguments"""
        g1 = ctrl.ss([], [], [], [[1, 2], [0, 3]])
        g2 = ctrl.ss([], [], [], [[1, 0], [2, 3]])
        ref = g2 * g1
        tst = ctrl.series(g1, g2)

        np.testing.assert_array_equal(ref.A, tst.A)
        np.testing.assert_array_equal(ref.B, tst.B)
        np.testing.assert_array_equal(ref.C, tst.C)
        np.testing.assert_array_equal(ref.D, tst.D)
예제 #21
0
def pid_design(G,
               K_guess,
               d_tc,
               verbose=False,
               use_P=True,
               use_I=True,
               use_D=True):
    # type: (control.tf, np.array, float, bool, bool, bool, bool) -> (np.array, control.tf, control.tf)
    """
    :param G: transfer function
    :param K_guess: gain matrix guess
    :param d_tc: time constant for derivative
    :param verbose: show debug output
    :param use_P: use p gain in design
    :param use_I: use i gain in design
    :param use_D: use d gain in design
    :return: (K, G_comp, Gc_comp)
        K: gain matrix
        G_comp: open loop compensated plant
        Gc_comp: closed loop compensated plant
    """
    # compensator transfer function
    H = []
    if use_P:
        H += [control.tf(1, 1)]
    if use_I:
        H += [control.tf((1), (1, 0))]
    if use_D:
        H += [control.tf((1, 0), (d_tc, 1))]
    H = np.array([H]).T
    H_num = [[H[i][j].num[0][0] for i in range(H.shape[0])]
             for j in range(H.shape[1])]
    H_den = [[H[i][j].den[0][0] for i in range(H.shape[0])]
             for j in range(H.shape[1])]
    H = control.tf(H_num, H_den)

    # print('G', G)
    # print('H', H)

    ss_open = control.tf2ss(G * H)

    if verbose:
        print('optimizing controller')
    K = lqr_ofb_design(K_guess, ss_open, verbose)
    if verbose:
        print('done')

    # print('K', K)
    # print('H', H)
    G_comp = control.series(G, H * K)
    Gc_comp = control.feedback(G_comp, 1)

    return K, G_comp, Gc_comp
예제 #22
0
def stepresponse(Kp, Ti, Td):
    G = Kp * control.tf([Ti * Td, Ti, 1], [Ti, 0])
    F = control.tf(1, [1, 6, 11, 6, 0])

    system = control.feedback(control.series(G, F), 1)
    t = numpy.arange(0, 100, 0.01)
    y, T = control.step(system, t)
    t_r, t_s, M_p = stepinfo(T, y)

    ISE = sum(numpy.power(numpy.subtract(y, 1), 2))

    return ISE, t_r, t_s, M_p
예제 #23
0
def f(X):

    Kp, Ki, Kd = X[0], X[1], X[2]

    # Controlador
    K = Kp + Ki / s + Kd * s / (1 + 0.001 * s)

    # Função de transferência de malha fechada r(t) -> [FTMF] -> y(t)
    FTMF = co.feedback(co.series(K, H))

    # Resposta ao degrau
    t1, y1 = co.step_response(FTMF, T=t, X0=0.0)
    # plt.plot(t1, y1, label='Resposta ao degrau')

    # Erro
    erro = 1 - y1
    # plt.plot(t1, erro**2, label='Erro quadrático')
    # plt.plot(t1, abs(erro), label='Erro absoluto')

    # Plot
    # plt.xlabel('Tempo(s)')
    # plt.ylabel('Amplitude')
    # plt.grid()
    # plt.legend()
    # plt.show()

    # u(t)
    t2, u1, xout = co.forced_response(K, T=t1, U=erro)
    # plt.plot(t2, u1, label='Sinal de controle')

    # Plot
    # plt.xlabel('Tempo(s)')
    # plt.ylabel('Amplitude')
    # plt.grid()
    # plt.legend()
    # x1,x2,y1,y2 = plt.axis()
    # plt.axis((x1,5,y1,y2))
    # plt.show()

    # Integral de erro quadrático
    I = sum(erro**2) * dt

    # Integral de erro absoluto
    # I = sum(abs(erro))*dt

    # Custo (Função LQR)
    Q = 1
    R = 0.01

    J = Q * I + R * sum(u1**2) * dt
    # print('\n', J)

    return J
예제 #24
0
    def test_lineariosys_statespace(self):
        """Make sure that a LinearIOSystem is also a StateSpace object"""
        iosys_siso = ct.LinearIOSystem(self.siso_linsys)
        self.assertTrue(isinstance(iosys_siso, ct.StateSpace))

        # Make sure that state space functions work for LinearIOSystems
        np.testing.assert_array_equal(
            iosys_siso.pole(), self.siso_linsys.pole())
        omega = np.logspace(.1, 10, 100)
        mag_io, phase_io, omega_io = iosys_siso.freqresp(omega)
        mag_ss, phase_ss, omega_ss = self.siso_linsys.freqresp(omega)
        np.testing.assert_array_equal(mag_io, mag_ss)
        np.testing.assert_array_equal(phase_io, phase_ss)
        np.testing.assert_array_equal(omega_io, omega_ss)

        # LinearIOSystem methods should override StateSpace methods
        io_mul = iosys_siso * iosys_siso
        self.assertTrue(isinstance(io_mul, ct.InputOutputSystem))

        # But also retain linear structure
        self.assertTrue(isinstance(io_mul, ct.StateSpace))

        # And make sure the systems match
        ss_series = self.siso_linsys * self.siso_linsys
        np.testing.assert_array_equal(io_mul.A, ss_series.A)
        np.testing.assert_array_equal(io_mul.B, ss_series.B)
        np.testing.assert_array_equal(io_mul.C, ss_series.C)
        np.testing.assert_array_equal(io_mul.D, ss_series.D)

        # Make sure that series does the same thing
        io_series = ct.series(iosys_siso, iosys_siso)
        self.assertTrue(isinstance(io_series, ct.InputOutputSystem))
        self.assertTrue(isinstance(io_series, ct.StateSpace))
        np.testing.assert_array_equal(io_series.A, ss_series.A)
        np.testing.assert_array_equal(io_series.B, ss_series.B)
        np.testing.assert_array_equal(io_series.C, ss_series.C)
        np.testing.assert_array_equal(io_series.D, ss_series.D)

        # Test out feedback as well
        io_feedback = ct.feedback(iosys_siso, iosys_siso)
        self.assertTrue(isinstance(io_series, ct.InputOutputSystem))

        # But also retain linear structure
        self.assertTrue(isinstance(io_series, ct.StateSpace))

        # And make sure the systems match
        ss_feedback = ct.feedback(self.siso_linsys, self.siso_linsys)
        np.testing.assert_array_equal(io_feedback.A, ss_feedback.A)
        np.testing.assert_array_equal(io_feedback.B, ss_feedback.B)
        np.testing.assert_array_equal(io_feedback.C, ss_feedback.C)
        np.testing.assert_array_equal(io_feedback.D, ss_feedback.D)
예제 #25
0
 def testMimoSeries(self):
     """regression: bdalg.series reverses order of arguments"""
     g1 = ctrl.ss([],[],[],[[1,2],[0,3]])
     g2 = ctrl.ss([],[],[],[[1,0],[2,3]])
     ref = g2*g1
     tst = ctrl.series(g1,g2)
     # assert_array_equal on mismatched matrices gives
     # "repr failed for <matrix>: ..."
     def assert_equal(x,y):
         np.testing.assert_array_equal(np.asarray(x),
                                       np.asarray(y))
     assert_equal(ref.A, tst.A)
     assert_equal(ref.B, tst.B)
     assert_equal(ref.C, tst.C)
     assert_equal(ref.D, tst.D)
예제 #26
0
 def testMimoSeries(self):
     """regression: bdalg.series reverses order of arguments"""
     g1 = ctrl.ss([],[],[],[[1,2],[0,3]])
     g2 = ctrl.ss([],[],[],[[1,0],[2,3]])
     ref = g2*g1
     tst = ctrl.series(g1,g2)
     # assert_array_equal on mismatched matrices gives
     # "repr failed for <matrix>: ..."
     def assert_equal(x,y):
         np.testing.assert_array_equal(np.asarray(x),
                                       np.asarray(y))
     assert_equal(ref.A, tst.A)
     assert_equal(ref.B, tst.B)
     assert_equal(ref.C, tst.C)
     assert_equal(ref.D, tst.D)
def step_response_BF_cor(num1, den1, num2, den2, feedback=1, t=None):

    if t == None:
        t = np.linspace(0, 10, 101)

    sys1 = ctl.tf(num1, den1)
    sys2 = ctl.tf(num2, den2)

    sys_BO = ctl.series(sys1, sys2)
    sys_BF = g = ctl.feedback(sys_BO, feedback)
    lti_BF = sys_BF.returnScipySignalLti()[0][0]

    t, s = signal.step2(lti_BF, None, t)

    return t, s
예제 #28
0
def func():
    import control

    sys1 = control.tf([
        2,
    ], [1, 0.1, 1])
    sys2 = control.tf([
        1,
    ], [1, 2])
    sys = control.series(sys1, sys2)
    t = range(100)
    t, u = control.step_response(control.tf([
        1,
    ], [10, 1]), t)
    t, y, x = control.forced_response(sys, t, u)
    h = ["u(t)", "y(t)"]
    return [u, y], t, h
예제 #29
0
def pid_design(G, K_guess, d_tc, verbose=False, use_P=True, use_I=True, use_D=True):
    """
    :param G: transfer function
    :param K_guess: gain matrix guess
    :param d_tc: time constant for derivative
    :param use_P: use p gain in design
    :param use_I: use i gain in design
    :param use_D: use d gain in design

    :return: (K, G_comp, Gc_comp)
        K: gain matrix
        G_comp: open loop compensated plant
        Gc_comp: closed loop compensated plant
    """
    # compensator transfer function
    H = []
    if use_P:
        H += [control.tf(1, 1)]
    if use_I:
        H += [control.tf((1), (1, 0))]
    if use_D:
        H += [control.tf((1, 0), (d_tc, 1))]
    H = np.array([H]).T
    H_num = [[H[i][j].num[0][0] for i in range(H.shape[0])] for j in range(H.shape[1])]
    H_den = [[H[i][j].den[0][0] for i in range(H.shape[0])] for j in range(H.shape[1])]
    H = control.tf(H_num, H_den)

    # print('G', G)
    # print('H', H)

    ss_open = control.tf2ss(G*H)

    if verbose:
        print('optimizing controller')
    K = lqr_ofb_design(K_guess, ss_open, verbose)
    if verbose:
        print('done')

    # print('K', K)
    # print('H', H)
    G_comp = control.series(G, H*K)
    Gc_comp = control.feedback(G_comp, 1)

    return K, G_comp, Gc_comp
예제 #30
0
def servo_loop(gain, tau):
    #      +
    # r----->O------C------G------->y
    #      - ^                 |
    #        |                 |
    #        |-------------H---|
    g = ctrl.series(
        ctrl.tf(np.array([1]), np.array([1, 0])),  # Integrator
        ctrl.tf(np.array([1]), np.array([0.1, 1])))  # Actuator lag
    h = ctrl.tf(np.array([1]), np.array([0.01, 1]))  # Sensor lag
    c = ctrl.tf(np.array([tau, 1]), np.array([0.008, 1])) * gain  # Control law
    fbsum = mat.ss([], [], [], [1, -1])
    sys_ol = mat.append(mat.tf2ss(c), mat.tf2ss(g), mat.tf2ss(h))
    sys_cl = mat.append(mat.tf2ss(c), mat.tf2ss(g), mat.tf2ss(h), fbsum)
    q_ol = np.array([[2, 1], [3, 2]])
    q_cl = np.array([[1, 4], [2, 1], [3, 2], [5, 3]])
    sys_ol = mat.connect(sys_ol, q_ol, [1], [3])
    sys_cl = mat.connect(sys_cl, q_cl, [4], [2])
    return mat.margin(sys_ol), sys_ol, sys_cl
예제 #31
0
def q1_perfFNC(Kp, Ti, Td):
    G = Kp * TransferFunction([Ti * Td, Ti, 1], [Ti, 0])
    F = TransferFunction(1, [1, 6, 11, 6, 0])
    sys = feedback(series(G, F), 1)
    sysinf = step_info(sys)

    t = []
    i = 0
    while i < 100:
        t.append(i)
        i += 0.01

    T, y = step_response(sys, T=t)
    ISE = sum((y - 1)**2)
    t_r = sysinf['RiseTime']
    t_s = sysinf['SettlingTime']
    M_p = sysinf['Overshoot']

    return ISE, t_r, t_s, M_p
예제 #32
0
def f(X):

    sis = tf([2], [4, 1])  #tf do sistema 2/(4s + 1)

    sisp = tf([X[0]], [1])  # tf do controlador proporcional Kp
    sisi = tf([X[0] * X[1]], [1, 0])  # tf do controlador integral Kp*Ki/s
    sisd = tf([X[0] * X[2], 0], [1])  # tf do controlador diferencial Kd*s

    sis2 = crt.parallel(sisp, sisi, sisd)  # tf do PID
    sis3 = crt.series(sis, sis2)  # tf do sistema em série com o PID

    mf = feedback(sis3, 1)  # malha fechada do sistema

    time = np.linspace(0, 20, 100)  # tempo de simulação
    _, y1 = step_response(mf,
                          time)  # resposta do sistema em malha fechada com PID

    erro = sum(abs(np.array((y1 - 1) * time)))  # integral do erro

    return erro
예제 #33
0
def unknown_step(Kp=None, Ki=None, Kd=None):
    """========================================================================
    Function: unknown_step() 
        generates a step response of the unkown system used in exercise 3 of 
        sheet 10. The response could either be openloop or with a PID 
        controller. For the PID controller you have to specify the Kp, Ki and 
        Kd gains.
    
    Synopsis:
        y,t = unknown_step()            # openloop
        y,t = unknown_step(Kp,Ki,Kd)    # with PID controller
    
    Input Parameter:
        - Kp: Proportional gain
        - Ki: Integral gain
        - Kd: Derivative gain
    
    Output Parameter:
        - y: The step response of the system
        - t: The time grid of the step response
        where y is actually t and vice versa
    ========================================================================"""
    t = np.linspace(0, 50, 100)
    # the system is unknown, DONT read it!
    sys = 5000 * ctrl.tf([1, 12, 32],
                         [1, 29, 829, 8939, 79418, 187280, 116000])
    sys = delay(sys, 2.1)
    # openloop if no K is specified
    if Kp == None or Ki == None or Kd == None:
        y, t = ctrl.step_response(sys, t)
        return y, t
    # closedloop PID
    else:
        PID = ctrl.tf([Kd, Kp, Ki], [1e-10, 1, 0])
        # the 1e-10 is a bit nasty but otherwise it cannot be turned into a
        # state space model, which is needed for the feedback to work proper.
        PIDss = ctrl.ss(PID)
        sysPID = ctrl.feedback(ctrl.series(PIDss, sys), 1)
        y, t = ctrl.step_response(sysPID, t)
        return y, t
def step_response_BF_pert(num1, den1, num2, den2, feedback=1, pert=0.25, pert_time=1, t=None):

    if t == None:
        t = np.linspace(0, 10, 101)

    sys1 = ctl.tf(num1, den1)
    sys2 = ctl.tf(num2, den2)

    sys_BO1 = ctl.series(sys1, sys2)
    sys_BF1 = g = ctl.feedback(sys_BO1, feedback)
    lti_BF1 = sys_BF1.returnScipySignalLti()[0][0]

    sys_BF2 = g = ctl.feedback(sys2, sys1)
    lti_BF2 = sys_BF2.returnScipySignalLti()[0][0]

    u = pert * (t > pert_time)

    t, s1 = signal.step2(lti_BF1, None, t)
    t, s2, trash = signal.lsim2(lti_BF2, u, t)

    s = s1 + s2
    return t, s
예제 #35
0
def motor_sim():
    # Simulation parameters
    setpoint = 100
    tmax = 20
    step = 100
    # Vehicule parameters
    m = 1000  # (kg)      vehicle mass
    b = 50  # (N.s/m)   damping coefficient
    # PI controller
    Kp = 300
    Ki = 15
    Cpi = control.tf([Kp, Ki], [1, 0])
    # Transfer function
    mot = control.tf([1], [m, b])
    gol = control.series(Cpi, mot)
    gcl = control.feedback(gol, 1)
    # Time
    t = np.linspace(0, tmax, step)
    # Input
    r = np.empty(len(t))
    r.fill(setpoint)
    # Output
    t, y, _ = control.forced_response(gcl, t, r)
    # Plot
    plt.figure()
    plt.title('Cruise Control Simulation')
    plt.axis([0, tmax + 1, 0, setpoint + 1])
    plt.grid()
    plt.xlabel('Time (s)')
    plt.ylabel('Speed (km/h)')
    # Animation + Sending message
    for i in range(step):
        y_int = int(round(y[i]))
        msg = can.Message(arbitration_id=100, data=[y_int, 0, 0, 0, 0, 0, 0])
        node0.send(msg)
        plt.scatter(t[i], y[i])
        plt.pause(OPTIONS['update_time'])
    plt.show()
def form_PI_cl(data):
    A = np.array([[1.0]])
    B = np.array([[1.0]])
    for i in range(N):
        C = np.array([[dt*data['Ki_fit'][i]]])
        D = np.array([[data['Kp_fit'][i]]])
        pi_block = ss(A, B, C, D)
        bike_block = ss(data['A_cl'][i], data['B_cl'][i], data['C_cl'][i], 0)
        pc = series(pi_block, bike_block)
        cl = feedback(pc, 1, sign=-1)

        data['yr_cl_evals'][i] = la.eigvals(cl.A)
        assert(np.all(abs(data['yr_cl_evals'][i]) < 1.0))
        data['A_yr_cl'][i] = cl.A
        data['B_yr_cl'][i] = cl.B
        data['C_yr_cl'][i] = cl.C
        assert(cl.D == 0)

        num, den = ss2tf(cl.A, cl.B, cl.C, cl.D)
        data['w_psi_r_to_psi_dot'][i], y = freqz(num[0], den)
        data['w_psi_r_to_psi_dot'][i] /= (dt * 2.0 * np.pi)
        data['mag_psi_r_to_psi_dot'][i] = 20.0 * np.log10(abs(y))
        data['phase_psi_r_to_psi_dot'][i] = np.unwrap(np.angle(y)) * 180.0 / np.pi
예제 #37
0
def attitude_rate_design(G, K_guess, d_tc):
    # compensator transfer function
    H = np.array([[
        control.tf(1, 1),
        control.tf((1), (1, 0)),
        control.tf((1, 0), (d_tc, 1))
    ]]).T
    H_num = [[H[i][j].num[0][0] for i in range(H.shape[0])]
             for j in range(H.shape[1])]
    H_den = [[H[i][j].den[0][0] for i in range(H.shape[0])]
             for j in range(H.shape[1])]
    H = control.tf(H_num, H_den)

    ss_open = control.tf2ss(G * H)

    print('optimizing controller')
    K = lqr_ofb_design(K_guess, ss_open)
    print('done')

    G_comp = control.series(G, H * K)
    Gc_comp = G_comp / (1 + G_comp)

    return K, G_comp, Gc_comp
예제 #38
0
def pidplot(num, den, kp, ki, kd):
    G = control.tf(num, den)
    if ki != 0:
        g2 = control.tf([kd, kp, ki], [1, 0])
    else:
        g2 = control.tf([kd, kp], [1])
    k = control.series(G, g2)
    s1 = "("
    s2 = "("
    s = str(G)
    s = s.split('\n')
    s[1] = s[1].strip()
    s[3] = s[3].strip()
    s1 = s1 + s[1] + ')'
    s2 = s2 + s[3] + ')'
    v = control.feedback(k, 1)
    t1, y1 = control.step_response(G)
    t2, y2 = control.step_response(v)
    plt.plot(t1, y1, 'b', linewidth=1, label='G(s)')
    plt.grid(True)
    plt.title('Time response of G(s)=' + (s1) + '/' + (s2))
    plt.xlabel('Time(sec)')
    plt.ylabel('Amplitude')
    if not os.path.isdir('static'):
        os.mkdir('static')
    else:
        # Remove old plot files
        for filename in glob.glob(
                os.path.join('static', 'pidcontrol', 'pidc.png')):
            os.remove(filename)

    pidc = os.path.join('static', 'pidcontrol', 'pidc.png')
    plt.savefig(pidc)
    plt.clf()
    plt.cla()
    plt.close()
    return pidc
예제 #39
0
def form_PI_cl(data):
    A = np.array([[1.0]])
    B = np.array([[1.0]])
    for i in range(N):
        C = np.array([[dt * data['Ki_fit'][i]]])
        D = np.array([[data['Kp_fit'][i]]])
        pi_block = ss(A, B, C, D)
        bike_block = ss(data['A_cl'][i], data['B_cl'][i], data['C_cl'][i], 0)
        pc = series(pi_block, bike_block)
        cl = feedback(pc, 1, sign=-1)

        data['yr_cl_evals'][i] = la.eigvals(cl.A)
        assert (np.all(abs(data['yr_cl_evals'][i]) < 1.0))
        data['A_yr_cl'][i] = cl.A
        data['B_yr_cl'][i] = cl.B
        data['C_yr_cl'][i] = cl.C
        assert (cl.D == 0)

        num, den = ss2tf(cl.A, cl.B, cl.C, cl.D)
        data['w_psi_r_to_psi_dot'][i], y = freqz(num[0], den)
        data['w_psi_r_to_psi_dot'][i] /= (dt * 2.0 * np.pi)
        data['mag_psi_r_to_psi_dot'][i] = 20.0 * np.log10(abs(y))
        data['phase_psi_r_to_psi_dot'][i] = np.unwrap(
            np.angle(y)) * 180.0 / np.pi
예제 #40
0
import control as co
import matplotlib.pyplot as plt
import numpy as np

s = co.tf('s')

a = 1 / (s - 3)
b = (s + 2) / (s - 2)
c = (s + 3) / (s + 4)
e = 1 / (s * ((s**2) + 1))

acp = co.parallel(c, a)
bap = co.parallel(b, a)
acbas = co.series(acp, bap)
acbaef = co.feedback(acbas, e, +1)

print(acbaef)

g_poles = co.pole(acbaef)
print("G poles")
for elem in g_poles:
    print(f'{elem: .2f}')

g_zeros = co.zero(acbaef)
print("G zeros")
for elem in g_zeros:
    print(f'{elem: .2f}')

g1_map = co.pzmap(acbaef, plot=True)

t = np.linspace(0, 25, 100)
def compute_gains(Q, R, W, V, dt):
    """Given LQR Q and R matrices, and Kalman W and V matrices, and sample
    time, compute optimal feedback gain and optimal filter gains."""

    data = np.empty((N,), dtype=controller_t)

    # Loop over all speeds for which we have system dynamics
    for i in range(N):
        data['theta_R_dot'][i] = theta_R_dot[i]
        data['dt'][i] = dt
        # Convert the bike dynamics to discrete time using a zero order hold
        data['A'][i], data['B'][i], _, _, _ = cont2discrete(
                        (A_w[i], B_w[i, :], eye(4), zeros((4, 1))), dt)
        data['plant_evals_d'][i] = la.eigvals(data['A'][i])
        data['plant_evals_c'][i] = np.log(data['plant_evals_d'][i]) / dt
        
        # Bicycle measurement matrices
        # - steer angle
        # - roll rate
        data['C_m'][i] = C_w[i, :2, :]
        # - yaw rate
        data['C_z'][i] = C_w[i, 2, :]

        A = data['A'][i]
        B = data['B'][i, :, 2].reshape((4, 1))
        C_m = data['C_m'][i]
        C_z = data['C_z'][i]

        # Controllability from steer torque
        data['ctrb_plant'][i] = ctrb(A, B)
        u, s, v = la.svd(data['ctrb_plant'][i])
        assert(np.all(s > 1e-13))

        # Solve discrete algebraic Ricatti equation associated with LQI problem
        P_c = dare(A, B, R, Q)
        
        # Optimal feedback gain using solution of Ricatti equation
        K_c = -la.solve(R + dot(B.T, dot(P_c, B)),
                                dot(B.T, dot(P_c, A)))
        data['K_c'][i] = K_c
        data['A_c'][i] = A + dot(B, K_c)
        data['B_c'][i] = B
        data['controller_evals'][i] = la.eigvals(data['A_c'][i])
        data['controller_evals_c'][i] = np.log(data['controller_evals'][i]) / dt
        assert(np.all(abs(data['controller_evals'][i]) < 1.0))

        # Observability from steer angle and roll rate measurement
        # Note that (A, C_m * A) must be observable in the "current estimator"
        # formulation
        data['obsv_plant'][i] = obsv(A, dot(C_m, A))
        u, s, v = la.svd(data['obsv_plant'][i])
        assert(np.all(s > 1e-13))

        # Solve Riccati equation
        P_e = dare(A.T, C_m.T, V, W)
        # Compute Kalman gain
        K_e = dot(P_e, dot(C_m.T, la.inv(dot(C_m, dot(P_e, C_m.T)) + V)))
        data['K_e'][i] = K_e
        data['A_e'][i] = dot(eye(4) - dot(K_e, C_m), A)
        data['B_e'][i] = np.hstack((dot(eye(4) - dot(K_e, C_m), B), K_e))
        data['estimator_evals'][i] = la.eigvals(data['A_e'][i])
        data['estimator_evals_c'][i] = np.log(data['estimator_evals'][i]) / dt
        # Verify that Kalman estimator eigenvalues are stable
        assert(np.all(abs(data['estimator_evals'][i]) < 1.0))

        # Closed loop state space equations
        A_cl = np.zeros((8, 8))
        A_cl[:4, :4] = A
        A_cl[:4, 4:] = dot(B, K_c)
        A_cl[4:, :4] = dot(K_e, dot(C_m, A))
        A_cl[4:, 4:] = A - A_cl[4:, :4] + A_cl[:4, 4:]
        data['A_cl'][i] = A_cl
        data['closed_loop_evals'][i] = la.eigvals(A_cl)
        assert(np.all(abs(data['closed_loop_evals'][i]) < 1.0))

        B_cl = np.zeros((8, 1))
        B_cl[:4, 0] = B.reshape((4,))
        B_cl[4:, 0] = dot(eye(4) - dot(K_e, C_m), B).reshape((4,))
        data['B_cl'][i] = B_cl

        C_cl = np.hstack((C_z, np.zeros((1, 4))))
        data['C_cl'][i] = C_cl

        # Transfer functions from r to yaw rate
        num, den = ss2tf(A_cl, B_cl, C_cl, 0)
        data['w_r_to_psi_dot'][i], y = freqz(num[0], den)
        data['w_r_to_psi_dot'][i] /= (dt * 2.0 * np.pi)
        data['mag_r_to_psi_dot'][i] = 20.0 * np.log10(abs(y))
        data['phase_r_to_psi_dot'][i] = np.unwrap(np.angle(y)) * 180.0 / np.pi

        # Open loop transfer function from e to yaw rate (PI loop not closed,
        # but LQR/LQG loop closed.
        inner_cl = ss(A_cl, B_cl, C_cl, 0)
        pi_block = ss([[1]], [[1]], [[data['Ki_fit'][i]*dt]], [[data['Kp_fit'][i]]])
        e_to_psi_dot = series(pi_block, inner_cl)
        num, den = ss2tf(e_to_psi_dot.A, e_to_psi_dot.B, e_to_psi_dot.C, e_to_psi_dot.D)
        data['w_e_to_psi_dot'][i], y = freqz(num[0], den)
        data['w_e_to_psi_dot'][i] /= (dt * 2.0 * np.pi)
        data['mag_e_to_psi_dot'][i] = 20.0 * np.log10(abs(y))
        data['phase_e_to_psi_dot'][i] = np.unwrap(np.angle(y)) * 180.0 / np.pi




    return data
예제 #42
0
from control.matlab import ss, c2d

from matlab_ext.plotter import *
from matlab_ext.r_ext import *

if __name__ == "__main__":
    cls()

    Kt = 1.0
    Jt = 1.0  # [?]

    u1 = tf([Kt], [1])
    u2 = tf([1 / Jt], [1])

    # fixme: как добавить сигнал шума?
    u12 = series(u1, u2)
    u3 = tf([1], [1, 0])
    u4 = copy.deepcopy(u3)
    u34 = series(u3, u4)

    # похоже нельзя соединить аналоговую и дискретную
    u34_d0 = c2d(u3, Ts=0.5)
    u34_d = c2d(u3, Ts=0.5)
    print series(u34_d0, u34_d)

    u14 = series(u12, u34)

    print u14

    ts, xs = step_response(u14)
#deriv = ctrl.tf ([Kd, Kp], [1,pd])
#C = ctrl.parallel (integ, deriv)

# Modelo del Controlador
Kp = 4; Kd = 2; Ki = .1
integ = ctrl.tf ([Ki],[1,0])
deriv = ctrl.tf ([Kd, 0], [0.1,1])
prop  = Kp

# Interconección del controlador
C = ctrl.parallel (deriv + prop, integ)

#derivPuro = ctrl.tf ([Kd, Kp], [0,1])

# Serie del Controlador y la Planta
l = ctrl.series (C,P)
# Realimentación Unitaria
sys = ctrl.feedback(l,1)

# Respuesta al escalón unitario
y,t = ctrl.matlab.step(sys, T = np.arange(0, Ttotal, DT) )

# Gráfica del escalón unitario
#mpl.plot(t,y)
#mpl.show()



#print y.shape, t.shape

#mag, phase, omega = bode(C)