PID controller for TR11 matches text.
This commit is contained in:
@@ -1,11 +1,9 @@
|
|||||||
class PIDController:
|
class PIDController:
|
||||||
def __init__(self, kp, ki, kd, d_filter_gain=0.1, imax=None, imin=None):
|
def __init__(self, kp, ki, kd, d_filter_gain=0.1):
|
||||||
self.kp = kp
|
self.kp = kp
|
||||||
self.ki = ki
|
self.ki = ki
|
||||||
self.kd = kd
|
self.kd = kd
|
||||||
self.d_filter_gain = d_filter_gain
|
self.d_filter_gain = d_filter_gain
|
||||||
self.imax = imax
|
|
||||||
self.imin = imin
|
|
||||||
self.reset()
|
self.reset()
|
||||||
|
|
||||||
def reset(self):
|
def reset(self):
|
||||||
@@ -15,10 +13,7 @@ class PIDController:
|
|||||||
|
|
||||||
def calculate(self, error, dt):
|
def calculate(self, error, dt):
|
||||||
self.integral += error * dt
|
self.integral += error * dt
|
||||||
if self.imax is not None and self.integral > self.imax:
|
|
||||||
self.integral = self.imax
|
|
||||||
if self.imin is not None and self.integral < self.imin:
|
|
||||||
self.integral = self.imin
|
|
||||||
# Add a low pass filter to the difference
|
# Add a low pass filter to the difference
|
||||||
difference = (error - self.error_prev) * self.d_filter_gain
|
difference = (error - self.error_prev) * self.d_filter_gain
|
||||||
self.error_prev += difference
|
self.error_prev += difference
|
||||||
|
|||||||
@@ -63,3 +63,6 @@ def set_left(speed):
|
|||||||
|
|
||||||
def set_right(speed):
|
def set_right(speed):
|
||||||
set_speed(right_motor, speed)
|
set_speed(right_motor, speed)
|
||||||
|
|
||||||
|
def send_line(message):
|
||||||
|
uart.write(f"{message}\n".encode())
|
||||||
|
|||||||
Reference in New Issue
Block a user