ecki/Robotics-at-Home-with-Raspberry-Pi-Pico
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Ch-8 cleanup

Browse Source
This commit is contained in:
Danny Staple 2022-03-20 22:28:27 +00:00
parent 36240090bf
commit 530f67856d
4 changed files with 89 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
ch-8/3-avoid/avoid_walls.py
View File

@ -11,33 +11,41 @@ robot.left_distance.start_ranging()
robot.right_distance.start_ranging() robot.right_distance.start_ranging()
try: try:
robot.set_left(speed) robot.set_left(speed)
robot.set_right(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() while True:
robot.right_distance.clear_interrupt() # do we have data:
time.sleep(0.1) 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: finally:
robot.stop() robot.stop()
robot.left_distance.clear_interrupt() robot.left_distance.clear_interrupt()
robot.right_distance.clear_interrupt() robot.right_distance.clear_interrupt()
robot.left_distance.stop_ranging() robot.left_distance.stop_ranging()
robot.right_distance.stop_ranging() robot.right_distance.stop_ranging()

40
ch-8/3-avoid/pio_encoder.py
View File

@ -54,24 +54,24 @@ send:
assembled = adafruit_pioasm.assemble(program) 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): class QuadratureEncoder:
while self.sm.in_waiting: def __init__(self, first_pin, second_pin, reversed=False):
self.sm.readinto(self._buffer) """Encoder with 2 pins. Must use sequential pins on the board"""
if self.reversed: self.sm = rp2pio.StateMachine(
return -self._buffer[0] assembled,
else: frequency=0,
return self._buffer[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]

8
ch-8/3-avoid/robot.py
View File

@ -29,6 +29,7 @@ def stop():
motor_B1.duty_cycle = 0 motor_B1.duty_cycle = 0
motor_B2.duty_cycle = 0 motor_B2.duty_cycle = 0
def set_speed(motor, speed): def set_speed(motor, speed):
# Swap motor pins if we reverse the speed # Swap motor pins if we reverse the speed
if speed < 0: if speed < 0:
@ -36,15 +37,16 @@ def set_speed(motor, speed):
speed = -speed speed = -speed
else: else:
direction = motor direction = motor
speed = min(speed, 1) # limit to 1.0 speed = min(speed, 1) # limit to 1.0
max_speed = 2**16-1 max_speed = 2 ** 16 - 1
direction[0].duty_cycle = int(max_speed * speed) direction[0].duty_cycle = int(max_speed * speed)
direction[1].duty_cycle = 0 direction[1].duty_cycle = 0
def set_left(speed): def set_left(speed):
set_speed(left_motor, speed) set_speed(left_motor, speed)
def set_right(speed): def set_right(speed):
set_speed(right_motor, speed) set_speed(right_motor, speed)

53
ch-8/3-avoid/varying_wall_avoid.py
View File

@ -1,4 +1,3 @@
import robot import robot
import time import time
@ -9,32 +8,38 @@ robot.right_distance.start_ranging()
speed = 0.9 speed = 0.9
def speed_from_distance(distance): def speed_from_distance(distance):
limited_distance = min(distance, 30) * speed limited_distance = min(distance, 30) * speed
return limited_distance / 30 return limited_distance / 30
try: try:
robot.set_left(speed) robot.set_left(speed)
robot.set_right(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() while True:
robot.right_distance.clear_interrupt() # do we have data:
time.sleep(0.1) 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: finally:
robot.stop() robot.stop()
robot.left_distance.clear_interrupt() robot.left_distance.clear_interrupt()
robot.right_distance.clear_interrupt() robot.right_distance.clear_interrupt()
robot.left_distance.stop_ranging() robot.left_distance.stop_ranging()
robot.right_distance.stop_ranging() robot.right_distance.stop_ranging()