126 lines
2.9 KiB
Python
126 lines
2.9 KiB
Python
import time
|
|
import board
|
|
import adafruit_hcsr04
|
|
from adafruit_motorkit import MotorKit
|
|
|
|
# Set the motors
|
|
kit = MotorKit()
|
|
m1 = kit.motor1
|
|
m2 = kit.motor2
|
|
m3 = kit.motor3
|
|
m4 = kit.motor4
|
|
|
|
# Set the sensor pins
|
|
front = adafruit_hcsr04.HCSR04(trigger_pin=board.D12, echo_pin=board.D13)
|
|
right = adafruit_hcsr04.HCSR04(trigger_pin=board.D10, echo_pin=board.D11)
|
|
left = adafruit_hcsr04.HCSR04(trigger_pin=board.D6, echo_pin=board.D9)
|
|
|
|
speed = 0.6
|
|
|
|
def forward():
|
|
m1.throttle = speed
|
|
m2.throttle = speed
|
|
m3.throttle = speed
|
|
m4.throttle = speed
|
|
|
|
def backward():
|
|
m1.throttle = -speed
|
|
m2.throttle = -speed
|
|
m3.throttle = -speed
|
|
m4.throttle = -speed
|
|
|
|
def stleft():
|
|
m1.throttle = -speed
|
|
m2.throttle = speed
|
|
m3.throttle = speed
|
|
m4.throttle = -speed
|
|
|
|
def stright():
|
|
m1.throttle = speed
|
|
m2.throttle = -speed
|
|
m3.throttle = -speed
|
|
m4.throttle = speed
|
|
|
|
def dgright():
|
|
m1.throttle =speed
|
|
m2.throttle = 0
|
|
m3.throttle = 0
|
|
m4.throttle =speed
|
|
|
|
def dgleft():
|
|
m1.throttle = 0
|
|
m2.throttle =speed
|
|
m3.throttle =speed
|
|
m4.throttle = 0
|
|
|
|
def turnright():
|
|
m1.throttle = speed
|
|
m2.throttle = speed
|
|
m3.throttle = speed
|
|
m4.throttle = 0
|
|
|
|
def turnleft():
|
|
m1.throttle = speed
|
|
m2.throttle = speed
|
|
m3.throttle = 0
|
|
m4.throttle = speed
|
|
|
|
def rotright():
|
|
m1.throttle = speed
|
|
m2.throttle = - speed
|
|
m3.throttle = 0
|
|
m4.throttle = 0
|
|
|
|
def rotleft():
|
|
m1.throttle = - speed
|
|
m2.throttle = speed
|
|
m3.throttle = 0
|
|
m4.throttle = 0
|
|
|
|
while True:
|
|
print("F=", f"{front.distance:.2f}", "R=", f"{right.distance:.2f}", "L=", f"{left.distance:.2f}")
|
|
try:
|
|
if front.distance <= 35: # What to do when there is an obstacle ahead
|
|
if left.distance > right.distance: # Diagonally left
|
|
dgleft()
|
|
time.sleep(0.8)
|
|
if right.distance > left.distance: # Diagonally right
|
|
dgright()
|
|
time.sleep(0.8)
|
|
|
|
if front.distance < 10:
|
|
backward()
|
|
time.sleep(0.7)
|
|
if left.distance > right.distance: # Diagonally left
|
|
rotleft()
|
|
time.sleep(0.5)
|
|
if right.distance > left.distance: # Diagonally right
|
|
rotright()
|
|
time.sleep(0.5)
|
|
|
|
if left.distance < 50: # Strafe right
|
|
turnright()
|
|
time.sleep(0.4)
|
|
if right.distance < 50: # Strafe left
|
|
turnleft()
|
|
time.sleep(0.4)
|
|
|
|
if left.distance < 15: # Strafe right
|
|
#turnright()
|
|
dgright()
|
|
time.sleep(0.5)
|
|
rotright()
|
|
time.sleep(0.4)
|
|
if right.distance < 15: # Strafe left
|
|
#turnleft()
|
|
dgleft()
|
|
time.sleep(0.5)
|
|
rotleft()
|
|
time.sleep(0.4)
|
|
|
|
|
|
else:
|
|
forward()
|
|
|
|
except RuntimeError:
|
|
pass
|