88 lines
No EOL
2 KiB
Python
88 lines
No EOL
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.D6, echo_pin=board.D9)
|
|
|
|
def forward():
|
|
m1.throttle = 0.5
|
|
m2.throttle = 0.5
|
|
m3.throttle = 0.5
|
|
m4.throttle = 0.5
|
|
|
|
def backward():
|
|
m1.throttle = -0.5
|
|
m2.throttle = -0.5
|
|
m3.throttle = -0.5
|
|
m4.throttle = -0.5
|
|
|
|
def stleft():
|
|
m1.throttle = -0.5
|
|
m2.throttle = 0.5
|
|
m3.throttle = 0.5
|
|
m4.throttle = -0.5
|
|
|
|
def stright():
|
|
m1.throttle = 0.5
|
|
m2.throttle = -0.5
|
|
m3.throttle = -0.5
|
|
m4.throttle = 0.5
|
|
|
|
def dgright():
|
|
m1.throttle =0.5
|
|
m2.throttle = 0
|
|
m3.throttle = 0
|
|
m4.throttle =0.5
|
|
|
|
def dgleft():
|
|
m1.throttle = 0
|
|
m2.throttle =0.5
|
|
m3.throttle =0.5
|
|
m4.throttle = 0
|
|
|
|
def turnright():
|
|
m1.throttle = 0.5
|
|
m2.throttle = 0.5
|
|
m3.throttle = 0.5
|
|
m4.throttle = 0
|
|
|
|
def turnleft():
|
|
m1.throttle = 0.5
|
|
m2.throttle = 0.5
|
|
m3.throttle = 0
|
|
m4.throttle = 0.5
|
|
|
|
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
|
|
dgleft()
|
|
time.sleep(0.8)
|
|
if right.distance > left.distance: # Diagonally right
|
|
dgright()
|
|
time.sleep(0.8)
|
|
if left.distance < 30: # Strafe right
|
|
turnright()
|
|
#stright()
|
|
time.sleep(0.5)
|
|
if right.distance < 30: # Strafe left
|
|
turnleft()
|
|
#stleft()
|
|
time.sleep(0.5)
|
|
else:
|
|
forward()
|
|
|
|
except RuntimeError:
|
|
pass |