diff --git a/ch-8/3-avoid/avoid_walls.py b/ch-8/3-avoid/avoid_walls.py index 7e0a540..2d031af 100644 --- a/ch-8/3-avoid/avoid_walls.py +++ b/ch-8/3-avoid/avoid_walls.py @@ -11,33 +11,41 @@ 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.set_left(speed) + robot.set_right(speed) - robot.left_distance.clear_interrupt() - robot.right_distance.clear_interrupt() - time.sleep(0.1) + 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() + 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 index e9202f8..ffcc9f2 100644 --- a/ch-8/3-avoid/pio_encoder.py +++ b/ch-8/3-avoid/pio_encoder.py @@ -54,24 +54,24 @@ send: 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 +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] diff --git a/ch-8/3-avoid/robot.py b/ch-8/3-avoid/robot.py index 1b339ef..a564a9b 100755 --- a/ch-8/3-avoid/robot.py +++ b/ch-8/3-avoid/robot.py @@ -29,6 +29,7 @@ def stop(): 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: @@ -36,15 +37,16 @@ def set_speed(motor, speed): speed = -speed else: direction = motor - speed = min(speed, 1) # limit to 1.0 - max_speed = 2**16-1 - + 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 index 16ff455..532c278 100644 --- a/ch-8/3-avoid/varying_wall_avoid.py +++ b/ch-8/3-avoid/varying_wall_avoid.py @@ -1,4 +1,3 @@ - import robot import time @@ -9,32 +8,38 @@ robot.right_distance.start_ranging() speed = 0.9 + def speed_from_distance(distance): - limited_distance = min(distance, 30) * speed - return limited_distance / 30 + 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.set_left(speed) + robot.set_right(speed) - robot.left_distance.clear_interrupt() - robot.right_distance.clear_interrupt() - time.sleep(0.1) + 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() + robot.stop() + robot.left_distance.clear_interrupt() + robot.right_distance.clear_interrupt() + robot.left_distance.stop_ranging() + robot.right_distance.stop_ranging()