123 lines
3 KiB
Python
123 lines
3 KiB
Python
import time
|
|
import random
|
|
from adafruit_crickit import crickit
|
|
|
|
LEFT = False
|
|
RIGHT = True
|
|
|
|
random.seed(int(time.monotonic()))
|
|
ss = crickit.seesaw
|
|
|
|
left_wheel = crickit.dc_motor_1
|
|
right_wheel = crickit.dc_motor_2
|
|
|
|
RIGHT_BUMPER = crickit.SIGNAL1
|
|
LEFT_BUMPER = crickit.SIGNAL2
|
|
CENTER_BUMPER = crickit.SIGNAL3
|
|
LBO = crickit.SIGNAL4
|
|
|
|
ss.pin_mode(RIGHT_BUMPER, ss.INPUT_PULLUP)
|
|
ss.pin_mode(LEFT_BUMPER, ss.INPUT_PULLUP)
|
|
ss.pin_mode(CENTER_BUMPER, ss.INPUT_PULLUP)
|
|
ss.pin_mode(LBO, ss.INPUT_PULLUP)
|
|
|
|
# These allow easy correction for motor speed variation.
|
|
# Factors are determined by observation and fiddling.
|
|
# Start with both having a factor of 1.0 (i.e. none) and
|
|
# adjust until the bot goes more or less straight
|
|
def set_right(speed):
|
|
right_wheel.throttle = speed * 0.9
|
|
|
|
def set_left(speed):
|
|
left_wheel.throttle = speed
|
|
|
|
|
|
# Uncomment this to find the above factors
|
|
# set_right(1.0)
|
|
# set_left(1.0)
|
|
# while True:
|
|
# pass
|
|
|
|
|
|
# Check for bumper activation and move away accordingly
|
|
# Returns False if we got clear, True if we gave up
|
|
def react_to_bumpers():
|
|
set_left(0.0)
|
|
set_right(0.0)
|
|
attempt_count = 0
|
|
|
|
# keep trying to back away and turn until we're free
|
|
while True:
|
|
|
|
# give up after 3 tries
|
|
if attempt_count == 3:
|
|
return True
|
|
|
|
bumped_left = not ss.digital_read(LEFT_BUMPER)
|
|
bumped_right = not ss.digital_read(RIGHT_BUMPER)
|
|
bumped_center = not ss.digital_read(CENTER_BUMPER)
|
|
|
|
# Didn't bump into anything, we're done here
|
|
if not bumped_left and not bumped_right and not bumped_center:
|
|
return False
|
|
|
|
# If the middle bumper was triggered, randomly pick a way to turn
|
|
if bumped_center:
|
|
bumped_left |= random.randrange(10) < 5
|
|
bumped_right = not bumped_left
|
|
|
|
# Back away a bit
|
|
set_left(-0.5)
|
|
set_right(-0.5)
|
|
time.sleep(0.5)
|
|
|
|
# If we bumped on the left, turn to the right
|
|
if bumped_left:
|
|
set_left(1.0)
|
|
set_right(0.0)
|
|
|
|
# If we bumped on the right, turn left
|
|
elif bumped_right:
|
|
set_left(0.0)
|
|
set_right(1.0)
|
|
|
|
# time to turn for
|
|
time.sleep(random.choice([0.2, 0.3, 0.4]))
|
|
attempt_count += 1
|
|
|
|
|
|
def tack(direction, duration):
|
|
target_time = time.monotonic() + duration
|
|
if direction == LEFT:
|
|
set_left(0.25)
|
|
set_right(1.0)
|
|
else:
|
|
set_left(1.0)
|
|
set_right(0.25)
|
|
while time.monotonic() < target_time:
|
|
if not(ss.digital_read(LEFT_BUMPER) and
|
|
ss.digital_read(RIGHT_BUMPER) and
|
|
ss.digital_read(CENTER_BUMPER)):
|
|
return react_to_bumpers()
|
|
return False
|
|
|
|
|
|
while True:
|
|
# check for low voltage/stall
|
|
if not ss.digital_read(LBO):
|
|
break
|
|
if tack(LEFT, 0.75):
|
|
break
|
|
if tack(RIGHT, 0.75):
|
|
break
|
|
|
|
set_left(0)
|
|
set_right(0)
|
|
|
|
while True:
|
|
for _ in range(3):
|
|
crickit.drive_2.fraction = 1.0
|
|
time.sleep(0.1)
|
|
crickit.drive_2.fraction = 0.0
|
|
time.sleep(.2)
|
|
time.sleep(10.0)
|