Ch-8 cleanup
This commit is contained in:
@@ -21,12 +21,20 @@ try:
|
|||||||
right_dist = robot.right_distance.distance
|
right_dist = robot.right_distance.distance
|
||||||
# are any too close
|
# are any too close
|
||||||
if right_dist < too_close_cm:
|
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)
|
robot.set_left(-speed)
|
||||||
else:
|
else:
|
||||||
robot.set_left(speed)
|
robot.set_left(speed)
|
||||||
if left_dist < too_close_cm:
|
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)
|
robot.set_right(-speed)
|
||||||
else:
|
else:
|
||||||
robot.set_right(speed)
|
robot.set_right(speed)
|
||||||
|
|||||||
@@ -54,6 +54,7 @@ send:
|
|||||||
|
|
||||||
assembled = adafruit_pioasm.assemble(program)
|
assembled = adafruit_pioasm.assemble(program)
|
||||||
|
|
||||||
|
|
||||||
class QuadratureEncoder:
|
class QuadratureEncoder:
|
||||||
def __init__(self, first_pin, second_pin, reversed=False):
|
def __init__(self, first_pin, second_pin, reversed=False):
|
||||||
"""Encoder with 2 pins. Must use sequential pins on the board"""
|
"""Encoder with 2 pins. Must use sequential pins on the board"""
|
||||||
@@ -62,10 +63,10 @@ class QuadratureEncoder:
|
|||||||
frequency=0,
|
frequency=0,
|
||||||
first_in_pin=first_pin,
|
first_in_pin=first_pin,
|
||||||
jmp_pin=second_pin,
|
jmp_pin=second_pin,
|
||||||
in_pin_count=2
|
in_pin_count=2,
|
||||||
)
|
)
|
||||||
self.reversed = reversed
|
self.reversed = reversed
|
||||||
self._buffer = array.array('i', [0])
|
self._buffer = array.array("i", [0])
|
||||||
|
|
||||||
def read(self):
|
def read(self):
|
||||||
while self.sm.in_waiting:
|
while self.sm.in_waiting:
|
||||||
@@ -74,4 +75,3 @@ class QuadratureEncoder:
|
|||||||
return -self._buffer[0]
|
return -self._buffer[0]
|
||||||
else:
|
else:
|
||||||
return self._buffer[0]
|
return self._buffer[0]
|
||||||
|
|
||||||
@@ -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:
|
||||||
@@ -37,14 +38,15 @@ def set_speed(motor, 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)
|
||||||
|
|||||||
@@ -1,4 +1,3 @@
|
|||||||
|
|
||||||
import robot
|
import robot
|
||||||
import time
|
import time
|
||||||
|
|
||||||
@@ -9,10 +8,12 @@ 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)
|
||||||
@@ -24,7 +25,11 @@ try:
|
|||||||
right_dist = robot.right_distance.distance
|
right_dist = robot.right_distance.distance
|
||||||
l_speed = speed_from_distance(robot.right_distance.distance)
|
l_speed = speed_from_distance(robot.right_distance.distance)
|
||||||
r_speed = speed_from_distance(robot.left_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_left(l_speed)
|
||||||
robot.set_right(r_speed)
|
robot.set_right(r_speed)
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user