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