from hcsr04 import HCSR04
from utime import sleep, sleep_ms
from machine import SoftI2C ,I2C, Pin, PWM
import ssd1306
from A4988 import A4988

# Setup I2C for ESP32
# different buses
i2c = SoftI2C(scl=Pin(22), sda=Pin(21), freq=400000, timeout = 500000)

oled_width = 128
oled_height = 64

oled = ssd1306.SSD1306_I2C(oled_width, oled_height, i2c)
stepper = A4988()
stepper2 = A4988(12, 13)

ultrasonic_sensor = HCSR04(trigger_pin=27, echo_pin=26)

led = Pin(14, Pin.OUT)

buzzer = PWM(4)

# min and max distance for ultrasinic to give warninh
A = 2
B = 70
# min and max value for sleep in ms
C = 100
D = 10000
distance = 0
   

def oled_display(oled, text):
    oled.fill(0) #clear screen
    oled.text(str(text),0 , 0)
    oled.show()

def cycle(dis, speed):
    stepper2.move_sync(800*4, speed)
    #time.sleep(0.0001)


while True:
    distance = ultrasonic_sensor.get_distance_cm()
    print(distance)

    oled_display(oled, distance)

    if distance <= 50:
        led.on()
        for i in range(5):
            cycle(800*4, 400)

    else:
        led.off()

        
    sleep(0.1)