#Ramy mhadhby 
from machine import Pin , Timer
import time
dir_pinL = Pin(16, Pin.OUT)
step_pinL = Pin(17, Pin.OUT)
dir_pinR = Pin(10, Pin.OUT)
step_pinR = Pin(11, Pin.OUT)

trig = Pin(1 , Pin.OUT)
echo = Pin(0 , Pin.IN)

def ultrasonic():
    trig.value(1)
    time.sleep_us(10)
    trig.value(0)
    while echo.value() == 0:
        pass
    pulse_start = time.ticks_us()
    while echo.value() == 1:
        pass
    pulse_end = time.ticks_us()
    pulse_duration = pulse_end - pulse_start
    distance = (pulse_duration * 34300) / 2 / 1000000
    # print(distance)
    return distance


def forward():
    print('forward')
    dir_pinR.value(0)
    dir_pinL.value(1)
    step_pinL.value(not step_pinL.value())
    step_pinR.value(not step_pinL.value())
    time.sleep(0.04)


def backward():
    print('backward')
    dir_pinR.value(1)
    dir_pinL.value(0)
    step_pinL.value(not step_pinL.value())
    step_pinR.value(not step_pinL.value())
    time.sleep(0.04)

def right():
    print('Right')
    dir_pinR.value(1)
    dir_pinL.value(1)
    step_pinL.value(not step_pinL.value())
    step_pinR.value(not step_pinL.value())
    time.sleep(0.04)
    
def left():
    print('left')
    dir_pinR.value(0)
    dir_pinL.value(0)
    step_pinL.value(not step_pinL.value())
    step_pinR.value(not step_pinL.value())
    time.sleep(0.04)

while True:
    d = ultrasonic()
    time.sleep(0.04)
    if d  > 50 :
        forward()
    else :
        for i in range(50):
            backward()
        for j in range(50) : 
            left()