Ch-8 cleanup

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

View File

@ -21,12 +21,20 @@ try:
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))
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))
print(
"Obstacle detected - Left: {} cm, Right: {} cm".format(
left_dist, right_dist
)
)
robot.set_right(-speed)
else:
robot.set_right(speed)

View File

@ -54,6 +54,7 @@ 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"""
@ -62,10 +63,10 @@ class QuadratureEncoder:
frequency=0,
first_in_pin=first_pin,
jmp_pin=second_pin,
in_pin_count=2
in_pin_count=2,
)
self.reversed = reversed
self._buffer = array.array('i', [0])
self._buffer = array.array("i", [0])
def read(self):
while self.sm.in_waiting:
@ -74,4 +75,3 @@ class QuadratureEncoder:
return -self._buffer[0]
else:
return self._buffer[0]

View File

@ -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:
@ -37,14 +38,15 @@ def set_speed(motor, speed):
else:
direction = motor
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[1].duty_cycle = 0
def set_left(speed):
set_speed(left_motor, speed)
def set_right(speed):
set_speed(right_motor, speed)

View File

@ -1,4 +1,3 @@
import robot
import time
@ -9,10 +8,12 @@ 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)
@ -24,7 +25,11 @@ try:
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))
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)