diff --git a/ch-11/1-measuring_wheel_speeds/code.py b/ch-11/1-measuring_wheel_speeds/code.py index 35f44f4..ae922cd 100644 --- a/ch-11/1-measuring_wheel_speeds/code.py +++ b/ch-11/1-measuring_wheel_speeds/code.py @@ -28,7 +28,7 @@ async def command_handler(): if uart.in_waiting: command = uart.readline().decode().strip() if command.startswith("M"): - speed = float(command[1:]) + Settings.speed = float(command[1:]) elif command.startswith("T"): Settings.time_interval = float(command[1:]) elif command == "O": @@ -43,5 +43,8 @@ async def command_handler(): await asyncio.sleep(3) await asyncio.sleep(0) -asyncio.create_task(motor_speed_loop()) -asyncio.run(command_handler()) +try: + asyncio.create_task(motor_speed_loop()) + asyncio.run(command_handler()) +finally: + robot.stop() diff --git a/ch-11/1-measuring_wheel_speeds/pio_encoder.py b/ch-11/1-measuring_wheel_speeds/pio_encoder.py index ffcc9f2..b831bf4 100644 --- a/ch-11/1-measuring_wheel_speeds/pio_encoder.py +++ b/ch-11/1-measuring_wheel_speeds/pio_encoder.py @@ -1,6 +1,8 @@ import rp2pio import adafruit_pioasm import array +import asyncio + program = """ ; use the osr for count @@ -67,10 +69,15 @@ class QuadratureEncoder: ) self.reversed = reversed self._buffer = array.array("i", [0]) + asyncio.create_task(self.poll_loop()) + + async def poll_loop(self): + while True: + await asyncio.sleep(0) + while self.sm.in_waiting: + self.sm.readinto(self._buffer) def read(self): - while self.sm.in_waiting: - self.sm.readinto(self._buffer) if self.reversed: return -self._buffer[0] else: diff --git a/ch-11/1-measuring_wheel_speeds/robot.py b/ch-11/1-measuring_wheel_speeds/robot.py index 4efeb56..70ebaef 100755 --- a/ch-11/1-measuring_wheel_speeds/robot.py +++ b/ch-11/1-measuring_wheel_speeds/robot.py @@ -7,22 +7,24 @@ import math wheel_diameter_mm = 70 wheel_circumference_mm = math.pi * wheel_diameter_mm -ticks_per_revolution = 2800 +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 -motor_A1 = pwmio.PWMOut(board.GP17, frequency=100) -motor_A2 = pwmio.PWMOut(board.GP16, frequency=100) -motor_B1 = pwmio.PWMOut(board.GP18, frequency=100) -motor_B2 = pwmio.PWMOut(board.GP19, frequency=100) +motor_A2 = pwmio.PWMOut(board.GP17, frequency=100) +motor_A1 = pwmio.PWMOut(board.GP16, frequency=100) +motor_B2 = pwmio.PWMOut(board.GP18, frequency=100) +motor_B1 = pwmio.PWMOut(board.GP19, frequency=100) right_motor = motor_A1, motor_A2 left_motor = motor_B1, motor_B2 -right_encoder = pio_encoder.QuadratureEncoder(board.GP20, board.GP21, reversed=True) -left_encoder = pio_encoder.QuadratureEncoder(board.GP26, board.GP27) +right_encoder = pio_encoder.QuadratureEncoder(board.GP20, board.GP21) +left_encoder = pio_encoder.QuadratureEncoder(board.GP26, board.GP27, reversed=True) i2c0 = busio.I2C(sda=board.GP0, scl=board.GP1) i2c1 = busio.I2C(sda=board.GP2, scl=board.GP3)