From 9e39796cf27e2daa05c2041e84d6539093877119 Mon Sep 17 00:00:00 2001 From: Danny Staple Date: Tue, 3 Jan 2023 22:05:12 +0000 Subject: [PATCH] Chapter 11 TR --- ch-11/2-speed-control/pid_controller.py | 1 - ch-11/3-known-distance/code.py | 23 ++++++++++++----------- ch-11/3-known-distance/pid_controller.py | 8 +------- 3 files changed, 13 insertions(+), 19 deletions(-) diff --git a/ch-11/2-speed-control/pid_controller.py b/ch-11/2-speed-control/pid_controller.py index 29b540e..b42e4ee 100644 --- a/ch-11/2-speed-control/pid_controller.py +++ b/ch-11/2-speed-control/pid_controller.py @@ -13,7 +13,6 @@ class PIDController: def calculate(self, error, dt): self.integral += error * dt - # Add a low pass filter to the difference difference = (error - self.error_prev) * self.d_filter_gain self.error_prev += difference diff --git a/ch-11/3-known-distance/code.py b/ch-11/3-known-distance/code.py index f5f47a5..33ef280 100644 --- a/ch-11/3-known-distance/code.py +++ b/ch-11/3-known-distance/code.py @@ -51,7 +51,7 @@ class DistanceTracker: expected = time_proportion * self.total_distance_in_ticks + self.current_position left.update(dt, expected) right.update(dt, expected) - robot.uart.write(f"0, {expected:.2f},{left.actual:.2f}\n".encode()) + robot.send_line(f"{expected:.2f},{left.actual:.2f},0") distance_tracker = DistanceTracker() @@ -61,23 +61,24 @@ async def command_handler(): while True: if robot.uart.in_waiting: command = robot.uart.readline().decode().strip() - # PID settings if command.startswith("M"): distance_tracker.speed = float(command[1:]) elif command.startswith("T"): distance_tracker.time_interval = float(command[1:]) - # Start/stop commands - elif command == "O": + elif command == "G": distance_tracker.set_distance(0) - elif command.startswith("O"): + elif command.startswith("G"): await asyncio.sleep(5) distance_tracker.set_distance(float(command[1:])) - # Print settings elif command.startswith("?"): - robot.uart.write(f"M{distance_tracker.speed:.1f}\n".encode()) - robot.uart.write(f"T{distance_tracker.time_interval:.1f}\n".encode()) + robot.send_line(f"M{distance_tracker.speed:.1f}") + robot.send_line(f"T{distance_tracker.time_interval:.1f}") await asyncio.sleep(3) await asyncio.sleep(0) - -asyncio.create_task(distance_tracker.loop()) -asyncio.run(command_handler()) + +try: + motors_task = asyncio.create_task(distance_tracker.loop()) + asyncio.run(command_handler()) +finally: + motors_task.cancel() + robot.stop() diff --git a/ch-11/3-known-distance/pid_controller.py b/ch-11/3-known-distance/pid_controller.py index a09358f..b42e4ee 100644 --- a/ch-11/3-known-distance/pid_controller.py +++ b/ch-11/3-known-distance/pid_controller.py @@ -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,6 @@ 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