import machine
import utime
#you could also set up the ultrasonic sensor like it is set up in the next couple comments
#from HCSR04 import HCSR04
#my_sensor = HCSR04()
#dist = my_sensor.cm()
#ultrasonic pins
trig = machine.Pin(13, machine.Pin.OUT)
#D7
echo = machine.Pin(5, machine.Pin.IN)
#D1
servo = machine.PWM(machine.Pin(12), freq=50)
#D6
dc_motor_forward = machine.Pin(0, machine.Pin.OUT)
#D3
dc_motor_backward = machine.Pin(2, machine.Pin.OUT)
#D4

#initial contitions

#cm = 0
#servo.duty(40)

#dist will return how far away something is from the sensor
def dist():
        trig.value(0)
        utime.sleep_us(2)
        trig.value(1)
        utime.sleep_us(10)
        trig.value(0)
        while echo.value() == 0:
                pass
        t1 = utime.ticks_us()
        while echo.value() == 0:
                pass
        t2 = utime.ticks_us()
        cm = (t2-t1) / 58.0
        print(cm,'cm')
        return cm       

#testing = dist()
#print(testing)

def move():
    for i in range(5):
        dc_motor_backward.value(0)
        dc_motor_forward.value(1)
        print("forward")
        utime.sleep(1)
        dc_motor_forward.value(0)
        dc_motor_backward.value(1)
        print("reverse")
        utime.sleep(1)
def brush():
    while True:
        check = dist()
        if check < 30 and check > 0:
            servo.duty(59)
            print("turn")
            print("brush")
            move()
            servo.duty(40)
            print("turn")
            print("brush brush brush")
            move()
            servo.duty(77)
            print("turn")
            print("brush brush brush brush brush")
            move()

brush()