-
Notifications
You must be signed in to change notification settings - Fork 0
/
PID.py
58 lines (46 loc) · 1.4 KB
/
PID.py
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
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@author : anthony
"""
import numpy as np
import math
class PID(object):
def __init__(self,KU = 1,KP = 1,KI = 0,KD = 0.1,TU = None,action_bound = [-1,1]):
self.KU = KU
self.TU = TU
self.KP = KP
self.KI = KI
self.KD = KD
self.Bound = action_bound
self.I = 0
def PID_ZN(self,KU,TU):
self.KU = KU
self.TU = TU
self.KP = 0.6 * KU
self.KI = 2 * self.KP / self.TU
self.KD = self.KP * TU / 8
def Pessen_Integral_ZN(self,KU,TU):
self.KU = KU
self.TU = TU
self.KP = 0.7 * KU
self.KI = 2.5 * self.KP / self.TU
self.KD = 0.15 * self.KP * self.TU
def Simple_overshoot(self,KU,TU):
self.KU = KU
self.TU = TU
self.KP = 0.33 * KU
self.KI = 2 * self.KP / TU
self.KD = self.KP * TU /3
def Update(self,error,derror,dt_Pid):
P = error * self.KP
self.I = self.I + error * self.KI * dt_Pid
D = derror * self.KD / dt_Pid
Out = P + self.I + D
if self.Bound != None:
Out = np.clip(Out,self.Bound[0],self.Bound[1])
return Out
def Clip(self,Input):
if self.Bound != None:
Input = np.clip(Input,self.Bound[0],self.Bound[1])
return Input