From 77e6e02703929b1af8dba72b62c8821be951e2ec Mon Sep 17 00:00:00 2001 From: Danny Staple Date: Tue, 22 Feb 2022 09:49:16 +0000 Subject: [PATCH] Chapter 8 code --- ch-8/1-tests/read_1_sensor.py | 19 +++++++ ch-8/2-two-sensors/read_2_sensors.py | 30 +++++++++++ ch-8/3-avoid/avoid_walls.py | 43 ++++++++++++++++ ch-8/3-avoid/pio_encoder.py | 77 ++++++++++++++++++++++++++++ ch-8/3-avoid/robot.py | 50 ++++++++++++++++++ ch-8/3-avoid/varying_wall_avoid.py | 40 +++++++++++++++ 6 files changed, 259 insertions(+) create mode 100644 ch-8/1-tests/read_1_sensor.py create mode 100644 ch-8/2-two-sensors/read_2_sensors.py create mode 100644 ch-8/3-avoid/avoid_walls.py create mode 100644 ch-8/3-avoid/pio_encoder.py create mode 100755 ch-8/3-avoid/robot.py create mode 100644 ch-8/3-avoid/varying_wall_avoid.py diff --git a/ch-8/1-tests/read_1_sensor.py b/ch-8/1-tests/read_1_sensor.py new file mode 100644 index 0000000..2ef64aa --- /dev/null +++ b/ch-8/1-tests/read_1_sensor.py @@ -0,0 +1,19 @@ +import time +import board +import busio +import adafruit_vl53l1x + +# i2c = busio.I2C(sda=board.GP0, scl=board.GP1) +i2c = busio.I2C(sda=board.GP2, scl=board.GP3) +vl53 = adafruit_vl53l1x.VL53L1X(i2c) + +vl53.distance_mode = 1 + +vl53.timing_budget = 100 +vl53.start_ranging() + +while True: + if vl53.data_ready: + print("Distance: {} cm".format(vl53.distance)) + vl53.clear_interrupt() + time.sleep(0.05) diff --git a/ch-8/2-two-sensors/read_2_sensors.py b/ch-8/2-two-sensors/read_2_sensors.py new file mode 100644 index 0000000..976fcf2 --- /dev/null +++ b/ch-8/2-two-sensors/read_2_sensors.py @@ -0,0 +1,30 @@ +import time +import board +import busio +import adafruit_vl53l1x + +i2c0 = busio.I2C(sda=board.GP0, scl=board.GP1) +i2c1 = busio.I2C(sda=board.GP2, scl=board.GP3) + +## If you accidentally pass the same bus twice, you'll get the same measurements +vl53_l = adafruit_vl53l1x.VL53L1X(i2c0) +vl53_r = adafruit_vl53l1x.VL53L1X(i2c1) + + +# we want it short range +vl53_l.distance_mode = 1 +vl53_l.timing_budget = 100 + +vl53_r.distance_mode = 1 +vl53_r.timing_budget = 100 + +vl53_l.start_ranging() +vl53_r.start_ranging() + + +while True: + if vl53_l.data_ready and vl53_r.data_ready: + print("Left: {} cm, Right: {} cm".format(vl53_l.distance, vl53_r.distance)) + vl53_l.clear_interrupt() + vl53_r.clear_interrupt() + time.sleep(0.05) diff --git a/ch-8/3-avoid/avoid_walls.py b/ch-8/3-avoid/avoid_walls.py new file mode 100644 index 0000000..7e0a540 --- /dev/null +++ b/ch-8/3-avoid/avoid_walls.py @@ -0,0 +1,43 @@ +import robot +import time + +robot.left_distance.distance_mode = 1 +robot.right_distance.distance_mode = 1 + +too_close_cm = 30 +speed = 0.9 + +robot.left_distance.start_ranging() +robot.right_distance.start_ranging() + +try: + robot.set_left(speed) + robot.set_right(speed) + + while True: + # do we have data: + if robot.left_distance.data_ready and robot.right_distance.data_ready: + left_dist = robot.left_distance.distance + right_dist = robot.right_distance.distance + # are any too close + if right_dist < too_close_cm: + print("Obstacle detected - Left: {} cm, Right: {} cm".format(left_dist, right_dist)) + robot.set_left(-speed) + else: + robot.set_left(speed) + if left_dist < too_close_cm: + print("Obstacle detected - Left: {} cm, Right: {} cm".format(left_dist, right_dist)) + robot.set_right(-speed) + else: + robot.set_right(speed) + + robot.left_distance.clear_interrupt() + robot.right_distance.clear_interrupt() + time.sleep(0.1) + +finally: + robot.stop() + robot.left_distance.clear_interrupt() + robot.right_distance.clear_interrupt() + robot.left_distance.stop_ranging() + robot.right_distance.stop_ranging() diff --git a/ch-8/3-avoid/pio_encoder.py b/ch-8/3-avoid/pio_encoder.py new file mode 100644 index 0000000..e9202f8 --- /dev/null +++ b/ch-8/3-avoid/pio_encoder.py @@ -0,0 +1,77 @@ +import rp2pio +import adafruit_pioasm +import array + +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]) + + def read(self): + while self.sm.in_waiting: + self.sm.readinto(self._buffer) + if self.reversed: + return -self._buffer[0] + else: + return self._buffer[0] + \ No newline at end of file diff --git a/ch-8/3-avoid/robot.py b/ch-8/3-avoid/robot.py new file mode 100755 index 0000000..e2fb14d --- /dev/null +++ b/ch-8/3-avoid/robot.py @@ -0,0 +1,50 @@ +import board +import pwmio +import pio_encoder +import busio +import adafruit_vl53l1x + + +motor_A1 = pwmio.PWMOut(board.GP17) +motor_A2 = pwmio.PWMOut(board.GP16) +motor_B1 = pwmio.PWMOut(board.GP18) +motor_B2 = pwmio.PWMOut(board.GP19) + +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) + +i2c0 = busio.I2C(sda=board.GP0, scl=board.GP1) +i2c1 = busio.I2C(sda=board.GP2, scl=board.GP3) + +left_distance = adafruit_vl53l1x.VL53L1X(i2c1) +right_distance = adafruit_vl53l1x.VL53L1X(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 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) diff --git a/ch-8/3-avoid/varying_wall_avoid.py b/ch-8/3-avoid/varying_wall_avoid.py new file mode 100644 index 0000000..16ff455 --- /dev/null +++ b/ch-8/3-avoid/varying_wall_avoid.py @@ -0,0 +1,40 @@ + +import robot +import time + +robot.left_distance.distance_mode = 1 +robot.left_distance.start_ranging() +robot.right_distance.distance_mode = 1 +robot.right_distance.start_ranging() + +speed = 0.9 + +def speed_from_distance(distance): + limited_distance = min(distance, 30) * speed + return limited_distance / 30 + +try: + robot.set_left(speed) + robot.set_right(speed) + + while True: + # do we have data: + if robot.left_distance.data_ready and robot.right_distance.data_ready: + left_dist = robot.left_distance.distance + right_dist = robot.right_distance.distance + l_speed = speed_from_distance(robot.right_distance.distance) + r_speed = speed_from_distance(robot.left_distance.distance) + print("Left: {} cm R Sp: {}, Right: {} cm L Sp: {}".format(left_dist, r_speed, right_dist, l_speed)) + robot.set_left(l_speed) + robot.set_right(r_speed) + + robot.left_distance.clear_interrupt() + robot.right_distance.clear_interrupt() + time.sleep(0.1) + +finally: + robot.stop() + robot.left_distance.clear_interrupt() + robot.right_distance.clear_interrupt() + robot.left_distance.stop_ranging() + robot.right_distance.stop_ranging()