from machine import Pin, ADC, PWM, I2C
import ssd1306
import time
import math  # Importar el módulo math

# === Configuración del hardware ===
# Configuración del potenciómetro
potenciometro = ADC(Pin(34))  # Conectar el potenciómetro al pin GPIO34
potenciometro.atten(ADC.ATTN_11DB)  # Configurar para leer 0-3.3V
potenciometro.width(ADC.WIDTH_12BIT)  # Resolución de 12 bits (0-4095)

# Configuración del servo motor
servo = PWM(Pin(25), freq=50)  # Conectar el servo al pin GPIO25, frecuencia de 50Hz

# Configuración de la pantalla OLED
i2c = I2C(0, scl=Pin(22), sda=Pin(21))  # Pines GPIO22 (SCL) y GPIO21 (SDA)
oled = ssd1306.SSD1306_I2C(128, 64, i2c)  # Pantalla OLED 128x64

# === Funciones auxiliares ===
def mapear(valor, in_min, in_max, out_min, out_max):
    """Mapea un valor de un rango a otro."""
    return int((valor - in_min) * (out_max - out_min) / (in_max - in_min) + out_min)

def actualizar_servo(posicion):
    """
    Actualiza la posición del servo.
    La posición debe estar en el rango de 0 a 180 grados.
    """
    # Convertir el ángulo (0-180) a un ciclo de trabajo PWM (duty) entre 40 y 115
    duty = mapear(posicion, 0, 180, 30, 120)
    servo.duty(duty)

def dibujar_servo(posicion):
    """
    Dibuja un gráfico del servo en la pantalla OLED.
    """
    oled.fill(0)  # Limpiar la pantalla
    oled.text("Servo Control", 0, 0)  # Título
    oled.text("Angulo: {} deg".format(posicion), 0, 10)  # Mostrar el ángulo

    # Dibujar el servo
    oled.rect(50, 30, 30, 10, 1)  # Base del servo
    brazo_x = 65 + int(20 * math.cos(math.radians(posicion - 90)))  # Coordenada X del brazo
    brazo_y = 35 - int(20 * math.sin(math.radians(posicion - 90)))  # Coordenada Y del brazo
    oled.line(65, 35, brazo_x, brazo_y, 1)  # Brazo del servo

    oled.show()  # Actualizar la pantalla

# === Bucle principal ===
while True:
    # Leer el valor del potenciómetro
    valor_pot = potenciometro.read()  # Valor entre 0 y 4095
    angulo = mapear(valor_pot, 0, 4095, 0, 180)  # Convertir a un ángulo entre 0 y 180 grados

    # Actualizar el servo y la pantalla
    actualizar_servo(angulo)
    dibujar_servo(angulo)

    time.sleep(0.1)  # Pequeño retardo para evitar actualizaciones demasiado rápidas