From b0db885480c3f52a9263f1aa97fc4d80f9d9a940 Mon Sep 17 00:00:00 2001 From: Danny Staple Date: Sun, 28 Aug 2022 20:39:32 +0100 Subject: [PATCH] Working speed control --- .../code.py | 34 +++++++++---------- .../pid_controller.py | 0 .../pio_encoder.py | 0 .../robot.py | 9 ++--- 4 files changed, 19 insertions(+), 24 deletions(-) rename ch-11/{2-driving-in-a-straight-line => 2-speed-control}/code.py (81%) rename ch-11/{2-driving-in-a-straight-line => 2-speed-control}/pid_controller.py (100%) rename ch-11/{2-driving-in-a-straight-line => 2-speed-control}/pio_encoder.py (100%) rename ch-11/{2-driving-in-a-straight-line => 2-speed-control}/robot.py (89%) diff --git a/ch-11/2-driving-in-a-straight-line/code.py b/ch-11/2-speed-control/code.py similarity index 81% rename from ch-11/2-driving-in-a-straight-line/code.py rename to ch-11/2-speed-control/code.py index 7d67c81..2684fe2 100644 --- a/ch-11/2-driving-in-a-straight-line/code.py +++ b/ch-11/2-speed-control/code.py @@ -1,23 +1,19 @@ import asyncio -import board -import busio -import robot import time +import robot import pid_controller -uart = busio.UART(board.GP12, board.GP13, baudrate=9600) - class Settings: - speed = 0.15 + speed = 0.17 time_interval = 0.2 motors_enabled = False class SpeedController: - def __init__(self, encoder, motor_fn) -> None: + def __init__(self, encoder, motor_fn): self.encoder = encoder self.motor_fn = motor_fn - self.pid = pid_controller.PIDController(1, 6, 0) + self.pid = pid_controller.PIDController(0, 6, 0) self.reset() def reset(self): @@ -48,20 +44,21 @@ async def motor_speed_loop(): await asyncio.sleep(Settings.time_interval) current_time = time.monotonic() dt = current_time - last_time + last_time = current_time left.update(dt) right.update(dt) - last_time = current_time - uart.write(f"0, {left.error:.2f},{right.error:.2f}\n".encode()) + robot.uart.write(f"0, {left.error:.2f},{right.error:.2f}\n".encode()) + async def stop_motors_after(seconds): await asyncio.sleep(seconds) Settings.motors_enabled = False - # robot.stop() + async def command_handler(): while True: - if uart.in_waiting: - command = uart.readline().decode().strip() + if robot.uart.in_waiting: + command = robot.uart.readline().decode().strip() # PID settings if command.startswith("P"): left.pid.kp = float(command[1:]) @@ -74,24 +71,25 @@ async def command_handler(): elif command.startswith("D"): left.pid.kd = float(command[1:]) right.pid.kd = float(command[1:]) + elif command.startswith("T"): + Settings.time_interval = float(command[1:]) # Speed settings elif command.startswith("M"): Settings.speed = float(command[1:]) - elif command.startswith("T"): - Settings.time_interval = float(command[1:]) # Start/stop commands elif command == "O": Settings.motors_enabled = False elif command.startswith("O"): + await asyncio.sleep(5) asyncio.create_task(stop_motors_after(float(command[1:]))) Settings.motors_enabled = True left.reset() right.reset() # Print settings elif command.startswith("?"): - uart.write(f"M{Settings.speed:.1f}\n".encode()) - uart.write(f"T{Settings.time_interval:.1f}\n".encode()) - uart.write(f"P{left.pid.kp:.2f}:I{left.pid.ki:.2f}:D{left.pid.kd:.2f}\n".encode()) + robot.uart.write(f"M{Settings.speed:.1f}\n".encode()) + robot.uart.write(f"P{left.pid.kp:.2f}:I{left.pid.ki:.2f}:D{left.pid.kd:.2f}\n".encode()) + robot.uart.write(f"T{Settings.time_interval:.1f}\n".encode()) await asyncio.sleep(3) await asyncio.sleep(0) diff --git a/ch-11/2-driving-in-a-straight-line/pid_controller.py b/ch-11/2-speed-control/pid_controller.py similarity index 100% rename from ch-11/2-driving-in-a-straight-line/pid_controller.py rename to ch-11/2-speed-control/pid_controller.py diff --git a/ch-11/2-driving-in-a-straight-line/pio_encoder.py b/ch-11/2-speed-control/pio_encoder.py similarity index 100% rename from ch-11/2-driving-in-a-straight-line/pio_encoder.py rename to ch-11/2-speed-control/pio_encoder.py diff --git a/ch-11/2-driving-in-a-straight-line/robot.py b/ch-11/2-speed-control/robot.py similarity index 89% rename from ch-11/2-driving-in-a-straight-line/robot.py rename to ch-11/2-speed-control/robot.py index 871a180..bc83e0b 100755 --- a/ch-11/2-driving-in-a-straight-line/robot.py +++ b/ch-11/2-speed-control/robot.py @@ -4,6 +4,9 @@ import pio_encoder import busio import adafruit_vl53l1x import math +import busio + +uart = busio.UART(board.GP12, board.GP13, baudrate=9600) wheel_diameter_mm = 70 wheel_circumference_mm = math.pi * wheel_diameter_mm @@ -22,7 +25,6 @@ motor_B1 = pwmio.PWMOut(board.GP19, frequency=100) right_motor = motor_A1, motor_A2 left_motor = motor_B1, motor_B2 -motor_dead_zone = 0.2 right_encoder = pio_encoder.QuadratureEncoder(board.GP20, board.GP21) left_encoder = pio_encoder.QuadratureEncoder(board.GP26, board.GP27, reversed=True) @@ -42,11 +44,6 @@ def stop(): def set_speed(motor, speed): - # stop completely if in the dead zone - if abs(speed) < motor_dead_zone: - motor[0].duty_cycle = 0 - motor[1].duty_cycle = 0 - return # Swap motor pins if we reverse the speed if speed < 0: direction = motor[1], motor[0]