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

def stleft():
    m1.throttle = -1
    m2.throttle = 1
    m3.throttle = 1
    m4.throttle = -1
    
def stright():
    m1.throttle = 1
    m2.throttle = -1
    m3.throttle = -1
    m4.throttle = 1

def dgright():
    m1.throttle = 1
    m2.throttle = 0
    m3.throttle = 0
    m4.throttle = 1

def dgleft():
    m1.throttle = 0
    m2.throttle = 1
    m3.throttle = 1
    m4.throttle = 0
    
def turnright():
    m1.throttle = 1
    m2.throttle = 1
    m3.throttle = 1
    m4.throttle = 0
    
def turnleft():
    m1.throttle = 1
    m2.throttle = 1
    m3.throttle = 0
    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
                dgleft()
            if right.distance > left.distance: # Diagonally right
                dgright()
        if left.distance < 30: # Strafe right
            turnright()
            #stright()
        if right.distance < 30: # Strafe left
            turnleft()
            #stleft()
        else:
            forward()

    except RuntimeError:
        pass