from machine import Pin, PWM, time_pulse_us, SoftI2C
from random import randint
from time import sleep_us, sleep
import ssd1306

# Inicializa los pines Trig y Echo
Trig = Pin(27, Pin.OUT)
Echo = Pin(26, Pin.IN)

#Inicializar PWM en el pin 15 para control del servo
servo = PWM(machine.Pin(15))
#Ajusta la frecuencia PWM
servo.freq (50)

# Inicializa la pantalla OLED
i2c = SoftI2C(scl=Pin(5), sda=Pin(4))
oled = ssd1306.SSD1306_I2C(128,64,i2c)

#LED RGB
LedRojo = PWM(Pin(0),5000)
LedVerde = PWM(Pin(1),5000)
LedAzul = PWM(Pin(2),5000)

#Buzzer
Buzzer = PWM(Pin(3))
Buzzer.duty_u16(0)
Buzzer.freq(500)

#Función para calcular el DC para cualquier ángulo:
def calcularDC(angulo):
    return (angulo*6062)//(180) + 1802
    
#Medición de la distancia
def medirDistancia():
    Trig.low()
    sleep_us(2)
    # Envia un pulso de 10us
    Trig.high()
    sleep_us(10)
    Trig.low()
    # Mide la duración del pulso en us
    duracion = time_pulse_us(Echo, Pin.high)
    # Calcula la distancia en cm
    Distancia = duracion/58.2
    print("Distancia:", Distancia)
    # Muestra la distancia en la pantalla OLED
    oled.fill(0)
    oled.text("Distancia:",20,20)
    oled.text(str(Distancia)+" cm",20,30)
    oled.show()
    sleep(0.01)    
    return Distancia

#Alarma
def Alarma():
    oled.text("INTRUSO",35,50)
    oled.show() 
    for i in range(5):
        Buzzer.duty_u16(1000)
        LedRojo.duty_u16(randint(0,65535))
        LedVerde.duty_u16(randint(0,65535))
        LedAzul.duty_u16(randint(0,65535))
        sleep(0.1)
        Buzzer.duty_u16(0)
        LedRojo.duty_u16(0)
        LedVerde.duty_u16(0)
        LedAzul.duty_u16(0) 
        sleep(0.1)

while True:
    for angulo in range(0,180,5):
        servo.duty_u16(calcularDC(angulo))
        Distancia = medirDistancia()
        if Distancia<30:
            Alarma()
    for angulo in range(180,0,-5):
        servo.duty_u16(calcularDC(angulo))
        Distancia = medirDistancia()   
        if Distancia<30:
            Alarma()
    sleep(0.01)