|
| 1 | +# SPDX-FileCopyrightText: 2018 Dave Astels for Adafruit Industries |
| 2 | +# |
| 3 | +# SPDX-License-Identifier: MIT |
| 4 | + |
| 5 | +import time |
| 6 | +import random |
| 7 | +from adafruit_crickit import crickit |
| 8 | + |
| 9 | +LEFT = False |
| 10 | +RIGHT = True |
| 11 | + |
| 12 | +random.seed(int(time.monotonic())) |
| 13 | +ss = crickit.seesaw |
| 14 | + |
| 15 | +left_wheel = crickit.dc_motor_1 |
| 16 | +right_wheel = crickit.dc_motor_2 |
| 17 | + |
| 18 | +RIGHT_BUMPER = crickit.SIGNAL1 |
| 19 | +LEFT_BUMPER = crickit.SIGNAL2 |
| 20 | +CENTER_BUMPER = crickit.SIGNAL3 |
| 21 | + |
| 22 | +ss.pin_mode(RIGHT_BUMPER, ss.INPUT_PULLUP) |
| 23 | +ss.pin_mode(LEFT_BUMPER, ss.INPUT_PULLUP) |
| 24 | +ss.pin_mode(CENTER_BUMPER, ss.INPUT_PULLUP) |
| 25 | + |
| 26 | +# These allow easy correction for motor speed variation. |
| 27 | +# Factors are determined by observation and fiddling. |
| 28 | +# Start with both having a factor of 1.0 (i.e. none) and |
| 29 | +# adjust until the bot goes more or less straight |
| 30 | +def set_right(speed): |
| 31 | + right_wheel.throttle = speed * 0.9 |
| 32 | + |
| 33 | +def set_left(speed): |
| 34 | + left_wheel.throttle = speed |
| 35 | + |
| 36 | + |
| 37 | +# Uncomment this to find the above factors |
| 38 | +# set_right(1.0) |
| 39 | +# set_left(1.0) |
| 40 | +# while True: |
| 41 | +# pass |
| 42 | + |
| 43 | +# Check for bumper activation and move away accordingly |
| 44 | +# Returns False if we got clear, True if we gave up |
| 45 | +def react_to_bumpers(): |
| 46 | + attempt_count = 0 |
| 47 | + # keep trying to back away and turn until we're free |
| 48 | + while True: |
| 49 | + |
| 50 | + # give up after 3 tries |
| 51 | + if attempt_count == 3: |
| 52 | + return True |
| 53 | + |
| 54 | + bumped_left = not ss.digital_read(LEFT_BUMPER) |
| 55 | + bumped_right = not ss.digital_read(RIGHT_BUMPER) |
| 56 | + bumped_center = not ss.digital_read(CENTER_BUMPER) |
| 57 | + |
| 58 | + # Didn't bump into anything, we're done here |
| 59 | + if not bumped_left and not bumped_right and not bumped_center: |
| 60 | + return False |
| 61 | + |
| 62 | + # If the middle bumper was triggered, randomly pick a way to turn |
| 63 | + if bumped_center: |
| 64 | + bumped_left |= random.randrange(10) < 5 |
| 65 | + bumped_right = not bumped_left |
| 66 | + |
| 67 | + # Back away a bit |
| 68 | + set_left(-0.5) |
| 69 | + set_right(-0.5) |
| 70 | + time.sleep(0.5) |
| 71 | + |
| 72 | + # If we bumped on the left, turn to the right |
| 73 | + if bumped_left: |
| 74 | + set_left(1.0) |
| 75 | + set_right(0.0) |
| 76 | + |
| 77 | + # If we bumped on the right, turn left |
| 78 | + elif bumped_right: |
| 79 | + set_left(0.0) |
| 80 | + set_right(1.0) |
| 81 | + |
| 82 | + # time to turn for |
| 83 | + time.sleep(random.choice([0.2, 0.3, 0.4])) |
| 84 | + attempt_count += 1 |
| 85 | + |
| 86 | + |
| 87 | +def tack(direction, duration): |
| 88 | + target_time = time.monotonic() + duration |
| 89 | + if direction == LEFT: |
| 90 | + set_left(0.25) |
| 91 | + set_right(1.0) |
| 92 | + else: |
| 93 | + set_left(1.0) |
| 94 | + set_right(0.25) |
| 95 | + while time.monotonic() < target_time: |
| 96 | + if not(ss.digital_read(LEFT_BUMPER) and |
| 97 | + ss.digital_read(RIGHT_BUMPER) and |
| 98 | + ss.digital_read(CENTER_BUMPER)): |
| 99 | + return react_to_bumpers() |
| 100 | + return False |
| 101 | + |
| 102 | + |
| 103 | +while True: |
| 104 | + if tack(LEFT, 0.75): |
| 105 | + break |
| 106 | + if tack(RIGHT, 0.75): |
| 107 | + break |
| 108 | + |
| 109 | + |
| 110 | +set_left(0) |
| 111 | +set_right(0) |
| 112 | + |
| 113 | +while True: |
| 114 | + for _ in range(3): |
| 115 | + crickit.drive_2.fraction = 1.0 |
| 116 | + time.sleep(0.1) |
| 117 | + crickit.drive_2.fraction = 0.0 |
| 118 | + time.sleep(.2) |
| 119 | + time.sleep(10.0) |
0 commit comments