-
Notifications
You must be signed in to change notification settings - Fork 17
/
pid.py
44 lines (35 loc) · 1.24 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
from time import time,sleep
class pid(object):
def __init__(self,kp,ki,kd,maxCorr=5):
self.kp = kp
self.ki = ki
self.kd = kd
self.previousTime = 0.0
self.I = 0
self.P =0
self.D = 0
self.previousError = 0.0
self.init=True
self.maxCorr=maxCorr
def calc(self, error):
if self.init:
#the first time do not calculate pid correction, but init the time data
self.previousTime = time()
self.init=False
return 0
else:
currentTime = time()
stepTime = currentTime - self.previousTime
self.P = error * self.kp
self.I += (error * stepTime) * self.ki
self.D = (error - self.previousError) / stepTime * self.kd
correction = self.P + self.I + self.D
self.previousTime = currentTime
self.previousError = error
#since W is an integer, correction is rounded
correction = round(correction)
if correction>self.maxCorr:
correction=self.maxCorr
if correction<-self.maxCorr:
correction=-self.maxCorr
return correction, self.P, self.I, self.D