#Nombre: Kevin Bermeo
#Fecha: 23/7/2025
#LECCIÓN MECATRÓNICA

from machine import Pin
import time
import dht
import utime

# Movimiento del dron
A = Pin(3, Pin.OUT)
izq = Pin(6, Pin.OUT)
der = Pin(7, Pin.OUT)
Ab = Pin(10, Pin.OUT)

# Temperatura
st = dht.DHT22(Pin(0))
a = Pin(1, Pin.OUT)

# Detección ultrasonido
trig = Pin(27, Pin.OUT)
echo = Pin(26, Pin.IN)

# Medidas
ve = 30
dir1 = 180
dir2 = 60

# Funciones de movimiento
def Arriba():
    A.on()
    der.off()
    izq.off()
    Ab.off()

def Derecha():
    A.off()
    der.on()
    izq.off()
    Ab.off()

def Izquierda():
    A.off()
    der.off()
    izq.on()
    Ab.off()

def ApagarMotores():
    A.off()
    der.off()
    izq.off()
    Ab.off()

def medir_distancia():
    trig.off()
    utime.sleep_us(2)
    trig.on()
    utime.sleep_us(10)
    trig.off()

    tiempo_inicial = utime.ticks_us()
    while echo.value() == 0:
        if utime.ticks_diff(utime.ticks_us(), tiempo_inicial) > 30000:
            return None
    start = utime.ticks_us()

    while echo.value() == 1:
        if utime.ticks_diff(utime.ticks_us(), start) > 30000:
            return None
    end = utime.ticks_us()

    duracion = utime.ticks_diff(end, start)
    distancia = (duracion / 2) / 29.1 
    return distancia

def verificar_temperatura():
    try:
        st.measure()
        temp = st.temperature()
        print(f"La Temperatura es: {temp}°C")
        if temp > 45:
            a.on()
        else:
            a.off()
    except:
        print("Error leyendo temperatura")

def recorrido_simple():
    movimientos = [
        (Derecha, dir1),
        (Arriba, dir2),
        (Izquierda, dir1),
        (Arriba, dir2),
        (Derecha, dir1),
        (Arriba, dir2),
        (Izquierda, dir1),
    ]
    for mov, distancia_en_cm in movimientos:
        mov()
        tiempo_mov = distancia_en_cm / ve
        start = time.ticks_ms()
        while time.ticks_diff(time.ticks_ms(), start) < tiempo_mov * 1000:
            distancia = medir_distancia()
            if distancia is not None and distancia < 30:
                print("Obstáculo detectado! Deteniendo movimiento.")
                ApagarMotores()
                return
            verificar_temperatura()
            time.sleep(0.1)
    ApagarMotores()

def main_loop():
    while True:
        recorrido_simple()

if __name__ == "__main__":
    main_loop()