46 lines
No EOL
1.2 KiB
Python
46 lines
No EOL
1.2 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.D8, echo_pin=board.D9)
|
|
|
|
def forward():
|
|
m1.throttle = 1
|
|
m2.throttle = 1
|
|
m3.throttle = 1
|
|
m4.throttle = 1
|
|
|
|
def backward():
|
|
m1.throttle = -1
|
|
m2.throttle = -1
|
|
m3.throttle = -1
|
|
m4.throttle = -1
|
|
|
|
while True:
|
|
print("F=", f"{front.distance:.2f}", "R=", f"{right.distance:.2f}", "L=", f"{left.distance:.2f}")
|
|
try:
|
|
if front.distance >= 20: # What to do when there is an obstacle ahead
|
|
if left.distance > right.distance: # Diagonally left
|
|
|
|
if right.distance > left.distance: # Diagonally right
|
|
|
|
if left.distance < 30: # Strafe right
|
|
|
|
if right.distance < 30: # Strafe left
|
|
|
|
else:
|
|
forward()
|
|
|
|
except RuntimeError:
|
|
pass |