# global right
# right = running_average.update(ultrasonic_distance.distance(trig3, echo3))
# time.sleep(SENSOR_DELAY)
if __name__ == '__main__':
try:
print("Starting")
#motor setup
motors.enable()
motors.setSpeeds(0, 0)
Lspeed = 0
Rspeed = 0
lst = [MIN_DIST] * WINDOW_LENGTH
#ultrasonic setup
running_average.average_init(lst)
ultrasonic_distance.init(trig1, echo1)
# ultrasonic_distance.init(trig2, echo2)
# ultrasonic_distance.init(trig3, echo3)
x = threading.Thread(target=ultrasonic1, args=())
x.daemon = True
x.start()
# y = threading.Thread(target=ultrasonic2, args=())
# y.daemon = True
# y.start()
# z = threading.Thread(target=ultrasonic3, args=())
# z.daemon = True
# z.start()
while True:
#turn if too close
if dist < MIN_DIST and TURNING == False:
print("TOO CLOSE")
ultrasonic_distance.distance(trig3, echo3))
time.sleep(0.05)
if __name__ == '__main__':
try:
print("Starting")
#motor setup
motors.enable()
motors.setSpeeds(0, 0)
Lspeed = 0
Rspeed = 0
lst = [MIN_DIST] * WINDOW_LENGTH
#ultrasonic setup
running_average.average_init(lst)
ultrasonic_distance.init(trig1, echo1)
ultrasonic_distance.init(trig2, echo2)
ultrasonic_distance.init(trig3, echo3)
x = threading.Thread(target=ultrasonics, args=())
x.daemon = True
x.start()
while True:
#turn if too close
if dist < MIN_DIST and TURNING == False:
print("TOO CLOSE")
TURNING = True
elif TURNING == True and (left <= WALL_DIST or right <= WALL_DIST):
TURNING = False
ACC = True
elif TURNING == True:
if Rspeed < Lspeed: