from machine import Pin, PWM, I2C
from time import sleep
import ssd1306
import radar_utilis  

servo = PWM(Pin(10))
servo.freq(50)

trigger = Pin(3, Pin.OUT)
echo = Pin(2, Pin.IN)

i2c = I2C(0, scl=Pin(1), sda=Pin(0))
oled = ssd1306.SSD1306_I2C(128, 64, i2c)

while True:
    radar_utilis.clear_radar(oled)  

    for angle in range(0, 181, 5):
        radar_utilis.move_servo(servo, angle)
        distance = radar_utilis.read_distance(trigger, echo)
        radar_utilis.plot_point(oled, angle, distance)

    for angle in range(180, -1, -5):
        radar_utilis.move_servo(servo, angle)
        distance = radar_utilis.read_distance(trigger, echo)
        radar_utilis.plot_point(oled, angle, distance)