- PID: Modified AWU code
This commit is contained in:
@@ -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
@@ -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
|
||||||
|
|||||||
Reference in New Issue
Block a user