From 3c47ff9874050d00b46f49ee4138eaed2168cdf0 Mon Sep 17 00:00:00 2001 From: Danny Staple Date: Sun, 11 Sep 2022 20:26:45 +0100 Subject: [PATCH] Current chapter 11 --- .../code.py | 4 +- .../pid_controller.py | 0 .../pio_encoder.py | 0 .../robot.py | 5 +- ch-11/2-speed-control/code.py | 2 +- ch-11/2-speed-control/robot.py | 5 +- .../code.py | 46 +++++++++---------- .../pid_controller.py | 0 .../pio_encoder.py | 0 .../robot.py | 9 +--- 10 files changed, 30 insertions(+), 41 deletions(-) rename ch-11/{1-measuring_wheel_speeds => 1-measuring-wheel-speeds}/code.py (89%) rename ch-11/{1-measuring_wheel_speeds => 1-measuring-wheel-speeds}/pid_controller.py (100%) rename ch-11/{1-measuring_wheel_speeds => 1-measuring-wheel-speeds}/pio_encoder.py (100%) rename ch-11/{1-measuring_wheel_speeds => 1-measuring-wheel-speeds}/robot.py (92%) rename ch-11/{3-known-distance-acceleration => 3-known-distance}/code.py (62%) rename ch-11/{3-known-distance-acceleration => 3-known-distance}/pid_controller.py (100%) rename ch-11/{3-known-distance-acceleration => 3-known-distance}/pio_encoder.py (100%) rename ch-11/{3-known-distance-acceleration => 3-known-distance}/robot.py (89%) diff --git a/ch-11/1-measuring_wheel_speeds/code.py b/ch-11/1-measuring-wheel-speeds/code.py similarity index 89% rename from ch-11/1-measuring_wheel_speeds/code.py rename to ch-11/1-measuring-wheel-speeds/code.py index 4d8fdfc..fa7e5e7 100644 --- a/ch-11/1-measuring_wheel_speeds/code.py +++ b/ch-11/1-measuring-wheel-speeds/code.py @@ -10,9 +10,9 @@ async def motor_speed_loop(): while True: await asyncio.sleep(Settings.time_interval) left_new, right_new = robot.left_encoder.read(), robot.right_encoder.read() - left_speed = robot.ticks_to_mm(left_new - left_last) / Settings.time_interval + left_speed = robot.ticks_to_m * (left_new - left_last) / Settings.time_interval left_last = left_new - right_speed = robot.ticks_to_mm(right_new - right_last) / Settings.time_interval + right_speed = robot.ticks_to_m * (right_new - right_last) / Settings.time_interval right_last = right_new robot.uart.write(f"{left_speed:.3f},{right_speed:.3f},0\n".encode()) diff --git a/ch-11/1-measuring_wheel_speeds/pid_controller.py b/ch-11/1-measuring-wheel-speeds/pid_controller.py similarity index 100% rename from ch-11/1-measuring_wheel_speeds/pid_controller.py rename to ch-11/1-measuring-wheel-speeds/pid_controller.py diff --git a/ch-11/1-measuring_wheel_speeds/pio_encoder.py b/ch-11/1-measuring-wheel-speeds/pio_encoder.py similarity index 100% rename from ch-11/1-measuring_wheel_speeds/pio_encoder.py rename to ch-11/1-measuring-wheel-speeds/pio_encoder.py diff --git a/ch-11/1-measuring_wheel_speeds/robot.py b/ch-11/1-measuring-wheel-speeds/robot.py similarity index 92% rename from ch-11/1-measuring_wheel_speeds/robot.py rename to ch-11/1-measuring-wheel-speeds/robot.py index bc83e0b..0c6a3d0 100755 --- a/ch-11/1-measuring_wheel_speeds/robot.py +++ b/ch-11/1-measuring-wheel-speeds/robot.py @@ -13,10 +13,7 @@ wheel_circumference_mm = math.pi * wheel_diameter_mm gear_ratio = 298 encoder_poles = 28 ticks_per_revolution = encoder_poles * gear_ratio -ticks_to_mm_const = wheel_circumference_mm / ticks_per_revolution - -def ticks_to_mm(ticks): - return ticks_to_mm_const * ticks +ticks_to_m = (wheel_circumference_mm / ticks_per_revolution) / 1000 motor_A2 = pwmio.PWMOut(board.GP17, frequency=100) motor_A1 = pwmio.PWMOut(board.GP16, frequency=100) diff --git a/ch-11/2-speed-control/code.py b/ch-11/2-speed-control/code.py index 26fc35b..74c0944 100644 --- a/ch-11/2-speed-control/code.py +++ b/ch-11/2-speed-control/code.py @@ -26,7 +26,7 @@ class SpeedController: current_ticks = self.encoder.read() speed_in_ticks = (current_ticks - self.last_ticks) / dt self.last_ticks = current_ticks - self.actual_speed = robot.ticks_to_mm(speed_in_ticks) / 1000 + self.actual_speed = robot.ticks_to_m * speed_in_ticks # calculate the error error = (Settings.speed * Settings.motors_enabled) - self.actual_speed # calculate the control signal diff --git a/ch-11/2-speed-control/robot.py b/ch-11/2-speed-control/robot.py index e653c39..b3f060b 100755 --- a/ch-11/2-speed-control/robot.py +++ b/ch-11/2-speed-control/robot.py @@ -13,10 +13,7 @@ wheel_circumference_mm = math.pi * wheel_diameter_mm gear_ratio = 298 encoder_poles = 28 ticks_per_revolution = encoder_poles * gear_ratio -ticks_to_mm_const = wheel_circumference_mm / ticks_per_revolution - -def ticks_to_mm(ticks): - return ticks_to_mm_const * ticks +ticks_to_m = (wheel_circumference_mm / ticks_per_revolution) / 1000 motor_A2 = pwmio.PWMOut(board.GP17, frequency=100) motor_A1 = pwmio.PWMOut(board.GP16, frequency=100) diff --git a/ch-11/3-known-distance-acceleration/code.py b/ch-11/3-known-distance/code.py similarity index 62% rename from ch-11/3-known-distance-acceleration/code.py rename to ch-11/3-known-distance/code.py index 2fd97f4..8ced3fb 100644 --- a/ch-11/3-known-distance-acceleration/code.py +++ b/ch-11/3-known-distance/code.py @@ -8,42 +8,41 @@ class DistanceController: def __init__(self, encoder, motor_fn): self.encoder = encoder self.motor_fn = motor_fn - # accel - # self.pid = pid_controller.PIDController(0.00000, 0, 0.00008, d_filter_gain=1) - self.pid = pid_controller.PIDController(0.00000, 0.0000, 0.00001, d_filter_gain=1) + # self.pid = pid_controller.PIDController(1.9, 0.5, 0.3, d_filter_gain=1) + self.pid = pid_controller.PIDController(3.25, 0.5, 0.5, d_filter_gain=1) + # works well with direct pwm control + # started with p at 0, and started increasing until it started to oscillate + # then reduced. + # started increasing i in 0.1 increments, to take up the steady state error + # once this was gone, there was still an overshoot. + # increase d in 0.1 increments until the overshoot was gone. self.start_ticks = self.encoder.read() - self.pwm = 0 self.error = 0 - def update(self, dt, expected, debug=False): - actual = self.encoder.read() - self.start_ticks + def update(self, dt, expected): + self.actual = self.encoder.read() - self.start_ticks # calculate the error - self.error = expected - actual - + self.error = (expected - self.actual) / robot.ticks_per_revolution # calculate the control signal control_signal = self.pid.calculate(self.error, dt) - print(control_signal) - # self.pwm += control_signal - if debug: - robot.uart.write(f"0, {expected:.2f},{actual:.2f}\n".encode()) - # self.motor_fn(self.pwm) self.motor_fn(control_signal) - + class DistanceTracker: def __init__(self): - self.speed = 0.10 + self.speed = 0.17 self.time_interval = 0.2 - self.reset() - - def reset(self): self.start_time = time.monotonic() + self.current_position = 0 self.total_distance_in_ticks = 0 self.total_time = 0.1 def set_distance(self, new_distance): - self.reset() - self.total_distance_in_ticks = robot.mm_to_ticks(new_distance * 1000) - self.total_time = new_distance / self.speed + # add the last travelled distance to the current position + self.current_position += self.total_distance_in_ticks + # calculate the new additional distance + self.total_distance_in_ticks = robot.m_to_ticks * new_distance + self.total_time = max(0.1, abs(new_distance / self.speed)) + self.start_time = time.monotonic() async def loop(self): left = DistanceController(robot.left_encoder, robot.set_left) @@ -56,9 +55,10 @@ class DistanceTracker: last_time = current_time elapsed_time = current_time - self.start_time time_proportion = min(1, elapsed_time / self.total_time) - expected = time_proportion * self.total_distance_in_ticks - left.update(dt, expected, debug=True) + 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()) distance_tracker = DistanceTracker() diff --git a/ch-11/3-known-distance-acceleration/pid_controller.py b/ch-11/3-known-distance/pid_controller.py similarity index 100% rename from ch-11/3-known-distance-acceleration/pid_controller.py rename to ch-11/3-known-distance/pid_controller.py diff --git a/ch-11/3-known-distance-acceleration/pio_encoder.py b/ch-11/3-known-distance/pio_encoder.py similarity index 100% rename from ch-11/3-known-distance-acceleration/pio_encoder.py rename to ch-11/3-known-distance/pio_encoder.py diff --git a/ch-11/3-known-distance-acceleration/robot.py b/ch-11/3-known-distance/robot.py similarity index 89% rename from ch-11/3-known-distance-acceleration/robot.py rename to ch-11/3-known-distance/robot.py index 673362b..fb885b9 100755 --- a/ch-11/3-known-distance-acceleration/robot.py +++ b/ch-11/3-known-distance/robot.py @@ -13,13 +13,8 @@ wheel_circumference_mm = math.pi * wheel_diameter_mm gear_ratio = 298 encoder_poles = 28 ticks_per_revolution = encoder_poles * gear_ratio -ticks_to_mm_const = wheel_circumference_mm / ticks_per_revolution - -def ticks_to_mm(ticks): - return ticks_to_mm_const * ticks - -def mm_to_ticks(mm): - return mm / ticks_to_mm_const +ticks_to_m = (wheel_circumference_mm / ticks_per_revolution) / 1000 +m_to_ticks = 1 / ticks_to_m motor_A2 = pwmio.PWMOut(board.GP17, frequency=100)