From 1b8533b459d75f0166fa593674985acba75dcea1 Mon Sep 17 00:00:00 2001 From: Danny Staple Date: Sun, 9 Oct 2022 22:33:18 +0100 Subject: [PATCH] Know turn behaviour for chapter 12. --- ch-12/3-always-face-north/code.py | 38 ++++------ ch-12/3-always-face-north/robot.py | 5 ++ ch-12/4-make-known-turn/code.py | 61 ++++++++++++++++ ch-12/4-make-known-turn/pid_controller.py | 27 ++++++++ ch-12/4-make-known-turn/pio_encoder.py | 84 +++++++++++++++++++++++ ch-12/4-make-known-turn/robot.py | 74 ++++++++++++++++++++ 6 files changed, 264 insertions(+), 25 deletions(-) create mode 100644 ch-12/4-make-known-turn/code.py create mode 100644 ch-12/4-make-known-turn/pid_controller.py create mode 100644 ch-12/4-make-known-turn/pio_encoder.py create mode 100755 ch-12/4-make-known-turn/robot.py diff --git a/ch-12/3-always-face-north/code.py b/ch-12/3-always-face-north/code.py index 6fd3acb..aec7837 100644 --- a/ch-12/3-always-face-north/code.py +++ b/ch-12/3-always-face-north/code.py @@ -19,8 +19,6 @@ class FaceNorthController: robot.set_left(control_signal) robot.set_right(-control_signal) - -# control loop async def control_loop(): controller = FaceNorthController() last_time = time.monotonic() @@ -34,30 +32,20 @@ async def control_loop(): controller.update(dt, angle) robot.uart.write(f"{angle}, 0\n".encode()) -# main async def main(): + while not robot.check_imu_status(): + await asyncio.sleep(0.1) + + robot.uart.write("Ready to go!\n".encode()) + # Wait for start signal + while True: + if robot.uart.in_waiting: + command = robot.uart.readline().decode().strip() + if command == "start": + break + await asyncio.sleep(0.1) + await control_loop() -def check_status(): - sys_status, gyro, accel, mag = robot.imu.calibration_status - print(f"Sys: {sys_status}, Gyro: {gyro}, Accel: {accel}, Mag: {mag}") - robot.uart.write(f"Sys: {sys_status}, Gyro: {gyro}, Accel: {accel}, Mag: {mag}\n".encode()) - return sys_status == 3 - - -while not check_status(): - time.sleep(0.1) - -print("Ready to go!") -robot.uart.write("Ready to go!\n".encode()) -# Wait for start signal -while True: - if robot.uart.in_waiting: - command = robot.uart.readline().decode().strip() - if command == "start": - break - else: - print("Unknown command: {}".format(command)) - time.sleep(0.1) - +print("Starting") asyncio.run(main()) diff --git a/ch-12/3-always-face-north/robot.py b/ch-12/3-always-face-north/robot.py index 847f5de..433d8ec 100755 --- a/ch-12/3-always-face-north/robot.py +++ b/ch-12/3-always-face-north/robot.py @@ -67,3 +67,8 @@ def set_left(speed): def set_right(speed): set_speed(right_motor, speed) + +def check_imu_status(): + sys_status, gyro, accel, mag = imu.calibration_status + uart.write(f"Sys: {sys_status}, Gyro: {gyro}, Accel: {accel}, Mag: {mag}\n".encode()) + return sys_status == 3 diff --git a/ch-12/4-make-known-turn/code.py b/ch-12/4-make-known-turn/code.py new file mode 100644 index 0000000..2bdc7a0 --- /dev/null +++ b/ch-12/4-make-known-turn/code.py @@ -0,0 +1,61 @@ +import robot +import pid_controller +import asyncio +import time + +class IMUTurnController: + def __init__(self): + self.pid = pid_controller.PIDController(0.01, 0.008, 0) + self.target = 0 + self.error = 0 + + def update(self, dt, angle): + error = self.target - angle + if error > 180: + error -= 360 + elif error < -180: + error += 360 + self.error = error + control_signal = self.pid.calculate(error, dt) + robot.set_left(control_signal) + robot.set_right(-control_signal) + + +async def command_handler(turn_controller): + while True: + if robot.uart.in_waiting: + command = robot.uart.readline().decode().strip() + if command.startswith("-"): + turn_controller.target -= int(command.lstrip('-')) + elif command.startswith("+"): + turn_controller.target += int(command.lstrip('+')) + elif command.isdigit(): + turn_controller.target += int(command) + await asyncio.sleep(0) + + +# control loop +async def control_loop(): + controller = IMUTurnController() + controller.target = robot.imu.euler[0] + asyncio.create_task(command_handler(controller)) + last_time = time.monotonic() + while True: + await asyncio.sleep(0.1) + next_time = time.monotonic() + dt = next_time - last_time + last_time = next_time + angle = robot.imu.euler[0] + + controller.update(dt, angle) + robot.uart.write(f"{controller.error}, 0\n".encode()) + + +async def main(): + while not robot.check_imu_status(): + await asyncio.sleep(0.1) + robot.uart.write("Ready to go!\n".encode()) + + await control_loop() + +asyncio.run(main()) diff --git a/ch-12/4-make-known-turn/pid_controller.py b/ch-12/4-make-known-turn/pid_controller.py new file mode 100644 index 0000000..a09358f --- /dev/null +++ b/ch-12/4-make-known-turn/pid_controller.py @@ -0,0 +1,27 @@ +class PIDController: + def __init__(self, kp, ki, kd, d_filter_gain=0.1, imax=None, imin=None): + 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): + self.integral = 0 + self.error_prev = 0 + self.derivative = 0 + + 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 + self.derivative = difference / dt + + return self.kp * error + self.ki * self.integral + self.kd * self.derivative diff --git a/ch-12/4-make-known-turn/pio_encoder.py b/ch-12/4-make-known-turn/pio_encoder.py new file mode 100644 index 0000000..b831bf4 --- /dev/null +++ b/ch-12/4-make-known-turn/pio_encoder.py @@ -0,0 +1,84 @@ +import rp2pio +import adafruit_pioasm +import array +import asyncio + + +program = """ +; use the osr for count +; input pins c1 c2 + + set y, 0 ; clear y + mov osr, y ; and clear osr +read: + ; x will be the old value + ; y the new values + mov x, y ; store old Y in x + in null, 32 ; Clear ISR - using y + in pins, 2 ; read two pins into y + mov y, isr + jmp x!=y, different ; Jump if its different + jmp read ; otherwise loop back to read + +different: + ; x has old value, y has new. + ; extract the upper bit of X. + in x, 31 ; get bit 31 - old p1 (remember which direction it came in) + in null, 31 ; keep only 1 bit + mov x, isr ; put this back in x + jmp !x, c1_old_zero + +c1_old_not_zero: + jmp pin, count_up + jmp count_down + +c1_old_zero: + jmp pin, count_down + ; fall through +count_up: + ; for a clockwise move - we'll add 1 by inverting + mov x, ~ osr ; store inverted OSR on x + jmp x--, fake ; use jump to take off 1 +fake: + mov x, ~ x ; invert back + jmp send +count_down: + ; for a clockwise move, just take one off + mov x, osr ; store osr in x + jmp x--, send ; dec and send +send: + ; send x. + mov isr, x ; send it + push noblock ; put ISR into input FIFO + mov osr, x ; put X back in OSR + jmp read ; loop back +""" + +assembled = adafruit_pioasm.assemble(program) + + +class QuadratureEncoder: + def __init__(self, first_pin, second_pin, reversed=False): + """Encoder with 2 pins. Must use sequential pins on the board""" + self.sm = rp2pio.StateMachine( + assembled, + frequency=0, + first_in_pin=first_pin, + jmp_pin=second_pin, + in_pin_count=2, + ) + 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): + if self.reversed: + return -self._buffer[0] + else: + return self._buffer[0] diff --git a/ch-12/4-make-known-turn/robot.py b/ch-12/4-make-known-turn/robot.py new file mode 100755 index 0000000..433d8ec --- /dev/null +++ b/ch-12/4-make-known-turn/robot.py @@ -0,0 +1,74 @@ +import board +import pwmio +import pio_encoder +import busio +import adafruit_vl53l1x +import math +import busio +import adafruit_bno055 + +uart = busio.UART(board.GP12, board.GP13, baudrate=9600) + +wheel_diameter_mm = 70 +wheel_circumference_mm = math.pi * wheel_diameter_mm +gear_ratio = 298 +encoder_poles = 28 +ticks_per_revolution = encoder_poles * gear_ratio +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) +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) +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) + +left_distance = adafruit_vl53l1x.VL53L1X(i2c0) +right_distance = adafruit_vl53l1x.VL53L1X(i2c1) +imu = adafruit_bno055.BNO055_I2C(i2c0) + +def stop(): + motor_A1.duty_cycle = 0 + motor_A2.duty_cycle = 0 + motor_B1.duty_cycle = 0 + motor_B2.duty_cycle = 0 + + +def set_speed(motor, speed): + # Swap motor pins if we reverse the speed + if abs(speed) < 0.1: + motor[0].duty_cycle = 0 + motor[1].duty_cycle = 1 + return + if speed < 0: + direction = motor[1], motor[0] + speed = -speed + else: + direction = motor + speed = min(speed, 1) # limit to 1.0 + max_speed = 2 ** 16 - 1 + + direction[0].duty_cycle = int(max_speed * speed) + direction[1].duty_cycle = 0 + + +def set_left(speed): + set_speed(left_motor, speed) + + +def set_right(speed): + set_speed(right_motor, speed) + +def check_imu_status(): + sys_status, gyro, accel, mag = imu.calibration_status + uart.write(f"Sys: {sys_status}, Gyro: {gyro}, Accel: {accel}, Mag: {mag}\n".encode()) + return sys_status == 3