132 lines
3.2 KiB
Python
132 lines
3.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)
|
|
|
|
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
|
|
|
|
# define strafing
|
|
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
|
|
|
|
# define diagonal movement
|
|
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
|
|
|
|
# define turning/steering
|
|
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
|
|
|
|
#define rotating
|
|
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
|
|
|
|
# Main control loop
|
|
while True:
|
|
print("F=", f"{front.distance:.2f}", "R=", f"{right.distance:.2f}", "L=", f"{left.distance:.2f}")
|
|
# The obstacle distance thresholds can be set however you want. I set them with the values I found to be optimal in folkrace
|
|
try:
|
|
if front.distance <= 35:
|
|
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: # Robot gets dangerously close to an obstacle or even hits it
|
|
# The robot will move backwards and rotare to the direction with the most space
|
|
backward()
|
|
time.sleep(0.7)
|
|
if left.distance > right.distance:
|
|
rotleft()
|
|
time.sleep(0.5)
|
|
if right.distance > left.distance:
|
|
rotright()
|
|
time.sleep(0.5)
|
|
|
|
if left.distance < 50: # Steer right
|
|
turnright()
|
|
time.sleep(0.4)
|
|
if right.distance < 50: # Steer left
|
|
turnleft()
|
|
time.sleep(0.4)
|
|
|
|
# When the robot's side gets dangerously close it moves diagonally and the rotates a bit to escape
|
|
if left.distance < 15:
|
|
dgright()
|
|
time.sleep(0.5)
|
|
rotright()
|
|
time.sleep(0.4)
|
|
if right.distance < 15:
|
|
dgleft()
|
|
time.sleep(0.5)
|
|
rotleft()
|
|
time.sleep(0.4)
|
|
|
|
|
|
else: # If no obstacle is seen, keep going forward
|
|
forward()
|
|
|
|
except RuntimeError:
|
|
pass
|