import machine
from machine import Pin, PWM ,I2C
from time import sleep
import ssd1306

i2c = I2C(0, scl=Pin(22), sda=Pin(21))
oled_width = 128
oled_height = 64
oled = ssd1306.SSD1306_I2C(oled_width, oled_height, i2c)

# Paso 1: Configurar los pines GPIO para los servos
p23 = machine.Pin(23, machine.Pin.OUT)  # Crear un objeto para el pin GPIO 23 como salida
p19 = machine.Pin(19, machine.Pin.OUT)  # Crear un objeto para el pin GPIO 19 como salida
p18 = machine.Pin(18, machine.Pin.OUT)  # Crear un objeto para el pin GPIO 18 como salida
p15 = machine.Pin(15, machine.Pin.OUT)  # Crear un objeto para el pin GPIO 5 como salida


# Paso 2: Configurar los objetos de servo y PWM
b_servo = machine.PWM(p23)  # Crear un objeto para el servo base y asociarlo al pin GPIO 23
s_servo = machine.PWM(p19)  # Crear un objeto para el servo secundario y asociarlo al pin GPIO 19
k_servo = machine.PWM(p18)  # Crear un objeto para el tercer servo  y asociarlo al pin GPIO 18
l_servo = machine.PWM(p15)  # Crear un objeto para el cuarto servo y asociarlo al pin GPIO 5

pulsador1 = Pin(13, Pin.IN, Pin.PULL_DOWN)
pulsador2 = Pin(14, Pin.IN, Pin.PULL_DOWN)
pulsador3 = Pin(27, Pin.IN, Pin.PULL_DOWN)
pulsador4 = Pin(26, Pin.IN, Pin.PULL_DOWN)
pulsador5 = Pin(25, Pin.IN, Pin.PULL_DOWN)

b_servo.init(freq=47, duty=25)  # Frecuencia de 50Hz, ciclo de trabajo inicial de 20%
s_servo.init(freq=47, duty=25)  # Frecuencia de 50Hz, ciclo de trabajo inicial de 20%
k_servo.init(freq=47, duty=25)  # Frecuencia de 50Hz, ciclo de trabajo inicial de 20%
l_servo.init(freq=47, duty=25)  # Frecuencia de 50Hz, ciclo de trabajo inicial de 20%

timb = machine.Timer(0)  # Crear un objeto temporizador para el servo base
tims = machine.Timer(1)  # Crear un objeto temporizador para el servo base
timk = machine.Timer(2)  # Crear un objeto temporizador para el servo base
timl = machine.Timer(3)  # Crear un objeto temporizador para el servo base

def handle_timb(timer):
    global is_timb
    if pulsador1.value() == 1:
        is_timb = True

def handle_tims(timer):
    global is_tims  # Acceder a la variable global "is_timB"
    if pulsador2.value() == 1:
        is_tims = True

def handle_timk(timer):
    global is_timk  # Acceder a la variable global "is_timB"
    if pulsador3.value() == 1:
        is_timk = True

def handle_timl(timer):
    global is_timl  # Acceder a la variable global "is_timB"
    if pulsador4.value() == 1:
        is_timl = True
   


timb.init(period=100, mode=machine.Timer.PERIODIC, callback=handle_timb)
tims.init(period=100, mode=machine.Timer.PERIODIC, callback=handle_tims)
timk.init(period=100, mode=machine.Timer.PERIODIC, callback=handle_timk)
timl.init(period=100, mode=machine.Timer.PERIODIC, callback=handle_timl)

is_timb = False  # Bandera para la interrupción del temporizador del servo base
is_tims = False  # Bandera para la interrupción del temporizador del servo base
is_timk = False  # Bandera para la interrupción del temporizador del servo base
is_timl = False  # Bandera para la interrupción del temporizador del servo base
b_angle = 0     # Ángulo actual del servo base
s_angle = 0    # Ángulo actual del servo secundario
k_angle = 0    # Ángulo actual del terecer servo
l_angle = 0    # Ángulo actual del cuarto servo

# Lista de ángulos a recorrer
angulos_1 = [0, 45, 90, 135, 30, 60, 15, 90]
angulos_2 = [60, 120, 30, 90, 0, 180, 45, 135]
angulos_3 = [0, 30, 75, 120, 15, 75, 90, 135]
angulos_4 = [45, 90, 0, 60, 30, 120, 15, 135]

def map(x, in_min, in_max, out_min, out_max):
    # Fórmula de mapeo:
    # (x - in_min) mapeado proporcionalmente al rango (out_max - out_min) y ajustado con out_min
    resultado = int((x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min)
    return resultado  # Devuelve el valor mapeado

def servo(pin, angle):
    # Llama a la función map para convertir el ángulo en un valor de ciclo de trabajo
    # El rango de entrada es de 0 a 180 grados, el rango de salida es de 20 a 120 (parámetros típicos para un servo)
    duty_cycle = map(angle, 0, 180, 25, 118)
    
    # Configura el ciclo de trabajo del pin PWM del servo
    pin.duty(duty_cycle)  


while True:

    if is_timb == True:
        servo(b_servo, b_angle)  # Mover el servo base a un nuevo ángulo
        for angle in angulos_1:
         # Mueve el servomotor al ángulo especificado
         servo(b_servo, angle)
         # Espera 1 segundo
         oled.fill(0)
         oled.text("Rotacion 1", 5, 10)
         oled.text("Servo 1", 10, 30)
         oled.text("Angulo =", 10, 40)
         oled.text(str(angle), 80, 40)
         oled.show()
         sleep(1.0)
    is_timb = 0

    if is_tims == True:
        servo(s_servo, s_angle)  # Mover el servo base a un nuevo ángulo
        for angle in angulos_2:
         # Mueve el servomotor al ángulo especificado
         servo(s_servo, angle)
         # Espera 1 segundo
         oled.fill(0)
         oled.text("Rotacion 2", 5, 10)
         oled.text("Servo 2", 10, 30)
         oled.text("Angulo =", 10, 40)
         oled.text(str(angle), 80, 40)
         oled.show()
         sleep(1.0)
    is_tims = 0

    if is_timk == True:
        servo(k_servo, k_angle)  # Mover el servo base a un nuevo ángulo
        for angle in angulos_3:
         # Mueve el servomotor al ángulo especificado
         servo(k_servo, angle)
         # Espera 1 segundo
         oled.fill(0)
         oled.text("Rotacion 3", 5, 10)
         oled.text("Servo 3", 10, 30)
         oled.text("Angulo =", 10, 40)
         oled.text(str(angle), 80, 40)
         oled.show()
         sleep(1.0)
    is_timk = 0

    if is_timl == True:
        servo(l_servo, l_angle)  # Mover el servo base a un nuevo ángulo
        for angle in angulos_4:
         # Mueve el servomotor al ángulo especificado
         servo(l_servo, angle)
         # Espera 1 segundo
         oled.fill(0)
         oled.text("Rotacion 4", 5, 10)
         oled.text("Servo 4", 10, 30)
         oled.text("Angulo =", 10, 40)
         oled.text(str(angle), 80, 40)
         oled.show()
         sleep(1.0)
    is_timl = 0

    if pulsador5.value():
        servo(b_servo, b_angle)
        servo(s_servo, s_angle)
        servo(k_servo, k_angle)
        servo(l_servo, l_angle)
      
        for angle in angulos_1:
         # Mueve el servomotor al ángulo especificado
         servo(b_servo, angle)
         oled.fill(0)
         oled.text("Rotacion 5", 5, 10)
         oled.text("Servo 1", 10, 30)
         oled.text("Angulo =", 10, 40)
         oled.text(str(angle), 80, 40)
         oled.show()
         sleep(0.5)

        for angle in angulos_2:
         # Mueve el servomotor al ángulo especificado
         servo(s_servo, angle)
         oled.fill(0)
         oled.text("Rotacion 5", 5, 10)
         oled.text("Servo 2", 10, 30)
         oled.text("Angulo =", 10, 40)
         oled.text(str(angle), 80, 40)
         oled.show()
         sleep(0.5)
     
        for angle in angulos_3:
         # Mueve el servomotor al ángulo especificado
         servo(k_servo, angle)
         oled.fill(0)
         oled.text("Rotacion 5", 5, 10)
         oled.text("Servo 3", 10, 30)
         oled.text("Angulo =", 10, 40)
         oled.text(str(angle), 80, 40)
         oled.show()
         sleep(0.5)
        
        for angle in angulos_4:
         # Mueve el servomotor al ángulo especificado
         servo(l_servo, angle)
         oled.fill(0)
         oled.text("Rotacion 5", 5, 10)
         oled.text("Servo 4", 10, 30)
         oled.text("Angulo =", 10, 40)
         oled.text(str(angle), 80, 40)
         oled.show()
         sleep(0.5)