PID controller for TR11 matches text.

This commit is contained in:
Danny Staple
2023-01-02 13:18:25 +00:00
parent d56b89787e
commit a9072b523e
2 changed files with 5 additions and 7 deletions
+2 -7
View File
@@ -1,11 +1,9 @@
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.ki = ki
self.kd = kd
self.d_filter_gain = d_filter_gain
self.imax = imax
self.imin = imin
self.reset()
def reset(self):
@@ -15,10 +13,7 @@ class PIDController:
def calculate(self, 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
difference = (error - self.error_prev) * self.d_filter_gain
self.error_prev += difference
+3
View File
@@ -63,3 +63,6 @@ def set_left(speed):
def set_right(speed):
set_speed(right_motor, speed)
def send_line(message):
uart.write(f"{message}\n".encode())