- PID: Modified AWU code

This commit is contained in:
jens
2020-12-01 23:21:45 +01:00
parent 5260af8bbd
commit d6037b33d3
2 changed files with 10 additions and 13 deletions
+1 -1
View File
@@ -227,7 +227,7 @@ if __name__ == '__main__':
"Hold": { "Hold": {
"Pid": { "Pid": {
"kp": 0.8, "kp": 0.8,
"ki": 0.00001, "ki": 0.002,
"kd": 0.0, "kd": 0.0,
"rho": 1.0 "rho": 1.0
} }
+9 -12
View File
@@ -31,23 +31,20 @@ class Pid:
kd = params['kd'] kd = params['kd']
rho = params['rho'] rho = params['rho']
yi = rho*self.yi + ki*dt * err self.yi = rho*self.yi + ki*dt * err
yd = err - self.err yd = kd/dt*(err - self.err)
yp = kp * err
_yp = kp * err
_yi = yi
_yd = kd/dt * yd
# Reset yi on error sign changes # Reset yi on error sign changes
if np.sign(err) != np.sign(self.err): if yp > self.y_max or yp < self.y_min:
_yi = 0 self.yi = 0
y = _yp + _yi + _yd self.err = err
y = yp + self.yi + yd
self.y = max(self.y_min, min(self.y_max, y)) self.y = max(self.y_min, min(self.y_max, y))
print("y = {}".format(_yi)) print("yi = {}".format(self.yi))
self.yi = yi
self.err = err
def get_y(self): def get_y(self):
return self.y return self.y