from machine import Pin, PWM, ADC
from time import sleep_ms

print("Hello, Pi Pico!")
print("Manejo de Sernvomotores con Joistick")
sleep_ms(1000)

# Inicialización de potenciómetros
potX = ADC(27)
potY = ADC(26)  # Corregido: potY debe estar en un pin diferente

# Inicialización de servos
servoY = PWM(Pin(15))
servoX = PWM(Pin(16))
servoX.freq(50)
servoY.freq(50)

# Función de mapeo de valores
def map_range(x, in_min, in_max, out_min, out_max):
    return int((x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min)

# Bucle principal
while True:
    valX = potX.read_u16()
    valY = potY.read_u16()

    pulsoX = map_range(valX, 0, 65535, 500000, 2500000)
    pulsoY = map_range(valY, 0, 65535 , 500000, 2500000)
    print("Val Pot X:", valX, "Val Pot Y:", valY)

    servoX.duty_ns(pulsoX)
    servoY.duty_ns(pulsoY)

    anguloX = map_range(pulsoX, 500000, 2500000, 0, 180)
    anguloY = map_range(pulsoY, 500000, 2500000, 0, 180)

    #print("Ángulo X:", anguloX, "Ángulo Y:", anguloY)
    sleep_ms(100)