-
Notifications
You must be signed in to change notification settings - Fork 0
/
PID.py
77 lines (57 loc) · 1.35 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
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
#! /usr/bin/python
# File name : PID.py
# Description : PID
# Website : www.adeept.com
# E-mail : [email protected]
# Author : William
# Date : 2019/02/23
import time
class PID:
def __init__(self):
self.Kp = 0
self.Kd = 0
self.Ki = 0
self.Initialize()
def SetKp(self,invar):
self.Kp = invar
def SetKi(self,invar):
self.Ki = invar
def SetKd(self,invar):
self.Kd = invar
def SetPrevError(self,preverror):
self.prev_error = preverror
def Initialize(self):
self.currtime = time.time()
self.prevtime = self.currtime
self.prev_error = 0
self.Cp = 0
self.Ci = 0
self.Cd = 0
def GenOut(self,error):
self.currtime = time.time()
dt = self.currtime - self.prevtime
de = error - self.prev_error
self.Cp = self.Kp*error
self.Ci += error*dt
self.Cd = 0
if dt > 0:
self.Cd = de/dt
self.prevtime = self.currtime
self.prev_error = error
return self.Cp + (self.Ki*self.Ci) + (self.Kd*self.Cd)
'''
pid = PID()
pid.SetKp(Kp)
pid.SetKd(Kd)
pid.SetKi(Ki)
fb = 0
outv = 0
PID_loop = True
while PID_loop:
error = Sp - fb
outv = pid.GenOut(error)
AnalogOut(outv)
time.sleep(0.05)
fb = AnalogIn(fb_input)
pass
'''