PID-RL/contained.py at main · ronaldli08/PID-RL · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
import numpy as np
import matplotlib.pyplot as plt
import scipy
import sympy
import control

s = control.TransferFunction.s #symbolic variable

#system to control
plant = 1/(s*(s+1)*(s+5))

Kp = 30
Ti = 10000000000
Td = 0
pid = Kp*(1 + 1/Ti*s + Td*s)

#closed loop system
feedback = control.feedback(plant*pid)
print(feedback)


# Routh's stability criterion from https://github.com/alchemyst/Dynamics-and-Control/blob/master/tbcontrol/symbolic.py
def routh(p):
    """ Construct the Routh-Hurwitz array given a polynomial in s

    Input: p - a sympy.Poly object
    Output: The Routh-Hurwitz array as a sympy.Matrix object
    """
    coefficients = p.all_coeffs()
    N = len(coefficients)
    M = sympy.zeros(N, (N+1)//2 + 1)

    r1 = coefficients[0::2]
    r2 = coefficients[1::2]
    M[0, :len(r1)] = [r1]
    M[1, :len(r2)] = [r2]
    for i in range(2, N):
        for j in range(N//2):
            S = M[[i-2, i-1], [0, j+1]]
            M[i, j] = sympy.simplify(-S.det()/M[i-1,0])
    return M[:, :-1]


#applying criterion
Kp = sympy.Symbol('Kp')
s = sympy.Symbol('s')

ce = s**3 + 6*s**2 + 5*s + Kp #charateristic equation

A = routh(sympy.Poly(ce,s))
A


# stability limits
sympy.solve([e > 0 for e in A[:,0]], Kp)


# getting roots
s = sympy.Symbol('s')
jw = sympy.Symbol('jw')
ce = s**3 + 6*s**2 + 5*s + 30

roots = sympy.solve(ce,s)
roots


# calculate Kp Ti Td
w = np.sqrt(5)
Pcr = (2*np.pi)/w

Kcr = 30
Kp = 0.6*Kcr
Ti = 0.5*Pcr
Td = 0.125*Pcr

print("Kp:", Kp)
print("Ti:", Ti)
print("Td:", Td)


s = control.TransferFunction.s

plant = (1/(s*(s+1)*(s+5)))

Kp = 18
Ti = 1.405
#Td = 0.351
Td = 0.5
pid = (Kp*(1 + 1/(Ti*s) + Td*s))

feedback = control.feedback(plant*pid)

t = np.linspace(0,17)
T, y = control.step_response(feedback, t)

print(feedback)

# plot
plt.plot(T, y)
plt.xlabel('Time (seconds)')
plt.ylabel('Output')
plt.title('Step Response')
plt.grid()
plt.show()