from machine import Pin, ADC, I2C
from pico_i2c_lcd import I2cLcd
from time import sleep
import math

# Configuración del servomotor y potenciómetro
from machine import PWM

servo = PWM(Pin(15))  # Conectar el servomotor al pin GP15
servo.freq(50)  # Frecuencia de 50 Hz para control de servos
servo1 = PWM(Pin(14))  # Conectar el servomotor al pin GP15
servo1.freq(50)  # Frecuencia de 50 Hz para control de servos
servo2 = PWM(Pin(13))  # Conectar el servomotor al pin GP15
servo2.freq(50)  # Frecuencia de 50 Hz para control de servos

pot = ADC(Pin(26))  # Potenciómetro conectado al pin GP26 (ADC)

# Configuración del LCD (con I2C)
I2C_ADDR = 0x27  # Dirección I2C del LCD (comúnmente 0x27 o 0x3F)
i2c = I2C(0, sda=Pin(0), scl=Pin(1), freq=400000)
lcd = I2cLcd(i2c, I2C_ADDR, 2, 16)

# Función para mover el servomotor en función de la posición mapeada
def mover_servo(valor1,valor2,valor3):
    # Mapeo de grados (0-180) a rango de pulso (500 a 2500 us)
    pulso = int(1800 + (angulo / 180) * 6060)
    pulso2 = int(1800 + (angulo2 / 180) * 6060)
    pulso3 = int(1800 + (angulo3 / 180) * 6060)
    servo.duty_u16(int(pulso))  # Convertir el pulso a 16 bits
    servo1.duty_u16(int(pulso2))  # Convertir el pulso a 16 bits
    servo2.duty_u16(int(pulso3))  # Convertir el pulso a 16 bits

    return pulso

# Función para leer y mapear el valor del potenciómetro
def leer_angulo(valor):
    # Mapea el valor leído (0-65535) a un ángulo (0-180)
    angulo = int(valor / 10 * 180)
    return angulo



numero = int(input("Escriba un número entre 0-10: "))
while  numero < 0 or numero > 10 :
    print("¡Ha escrito un número no disponible! Inténtelo de nuevo")
    numero = int(input("Escriba un número positivo: "))

print("AZUL: ",numero)
azul=numero
numero = int(input("Escriba un número entre 0-10: "))
while  numero < 0 or numero > 10 :
    print("¡Ha escrito un número no disponible! Inténtelo de nuevo")
    numero = int(input("Escriba un número positivo: "))

print("ROJO: ",numero)
rojo=numero
numero = int(input("Escriba un número entre 0-10: "))
while  numero < 0 or numero > 10 :
    print("¡Ha escrito un número no disponible! Inténtelo de nuevo")
    numero = int(input("Escriba un número positivo: "))

print("AMARILLO: ",numero)
amarillo=numero
numero = float(input("Escriba un número entre 0-10: "))
while  numero < 0.1 or numero > 5 :
    print("¡Ha escrito un número no disponible! Inténtelo de nuevo")
    numero = float(input("Escriba un número positivo: "))
    
print("PESO KG: ",numero)
peso=numero

print(azul,rojo,amarillo,peso)
# Bucle principal
while True:
    angulo = leer_angulo(azul)  # Leer el ángulo del potenciómetro
    angulo2 = leer_angulo(rojo)  # Leer el ángulo del potenciómetro
    angulo3 = leer_angulo(amarillo)  # Leer el ángulo del potenciómetro

    mover_servo(angulo,angulo2,angulo3)  # Mover el servomotor al ángulo calculado
    
    print (angulo,angulo2,angulo3)
    pot_value = pot.read_u16()  # Leer valor del potenciómetro (0-65535)
    

    # Mostrar el ángulo en el LCD
    lcd.clear()
    lcd.putstr("Angulo: {:3d} grados".format(angulo))
    
    sleep(0.1)  # Pequeña pausa antes de la siguiente lectura