import machine
import time
import math

#Configuración del sensor de distancia ultrasónico
trig = machine.Pin(16, machine.Pin.OUT)
echo = machine.Pin(17, machine.Pin.IN)

#Configuración del servo motor
servo = machine.PWM(machine.Pin(15))
servo.freq(50)

#Configuración del motor a pasos
motor_pins = [
    machine.Pin(18, machine.Pin.OUT),
    machine.Pin(19, machine.Pin.OUT),
    machine.Pin(20, machine.Pin.OUT),
    machine.Pin(21, machine.Pin.OUT)
]

#Secuencia de pasos para el motor a pasos
step_sequence = [
    [1, 0, 0, 0],
    [1, 1, 0, 0],
    [0, 1, 0, 0],
    [0, 1, 1, 0],
    [0, 0, 1, 0],
    [0, 0, 1, 1],
    [0, 0, 0, 1],
    [1, 0, 0, 1]
]

#Función para medir la distancia
def medir_distancia():
    trig.value(1)
    time.sleep_us(10)
    trig.value(0)
    while echo.value() == 0:
        pass
    inicio = time.ticks_us()
    while echo.value() == 1:
        pass
    fin = time.ticks_us()
    distancia = (fin - inicio) / 58
    return distancia

#Función para mover el servo motor
def mover_servo(angulo):
    duty = int((angulo / 180) * 1024 + 1024 / 4)
    servo.duty_u16(duty)

#Función para girar el motor a pasos
def girar_motor(pasos):
    for i in range(pasos):
        for step in step_sequence:
            for j in range(4):
                motor_pins[j].value(step[j])
            time.sleep_us(1000)

#Función para escanear el entorno
def escanear():
    puntos = []
    for angulo in range(0, 180, 10):
        mover_servo(angulo)
        time.sleep_ms(500)
        girar_motor(10)
        time.sleep_ms(500)
        distancia = medir_distancia()
        puntos.append((angulo, distancia))
    return puntos

#Main
while True:
    puntos = escanear()
    for punto in puntos:
        print(f"Ángulo: {punto[0]}, Distancia: {punto[1]}")
    time.sleep_ms(1000)