#Ejemplo simulando el control de dos servos para alerones ala Zagi

from machine import Pin, PWM, ADC
from utime import sleep, sleep_ms
freq = 50
start_angle = 90
start_angle_l = 90
start_angle_r= 90
servo_left = PWM(Pin(26), freq)
servo_right = PWM(Pin(21), freq)

#switch = ADC(Pin(13))
#sw_value = switch.read()
#print(sw_value)


axis_x = ADC(Pin(14))
axis_y = ADC(Pin(27))

#Atenuación de esp a 3,3 volts en x y y.

axis_x.atten(ADC.ATTN_11DB)
axis_y.atten(ADC.ATTN_11DB)

#Resolución 4096 8 bits
axis_x.width(ADC.WIDTH_9BIT)
axis_y.width(ADC.WIDTH_9BIT)




while True:

    y_value = axis_y.read()
    x_value = axis_x.read()

    
    servo_left.duty(start_angle)
    servo_right.duty(start_angle)
    print(x_value," ",y_value)
    sleep_ms(5)
    i = start_angle

    #Giros derecha e izquierda
    if x_value==0:

        for i in range(start_angle,60,-1):
            servo_left.duty(i)
            sleep_ms(40)

        for i in range(start_angle,120):
            servo_right.duty(i)
            sleep_ms(40)
    
    if x_value==511:
        for i in range(start_angle,120):
            servo_left.duty(i)
            sleep_ms(40)
        
        for i in range(start_angle,60,-1):
            servo_right.duty(i)
            sleep_ms(40)

    #ascenso y descenso


    if y_value==0:
        for i in range(start_angle,60,-1):
            servo_left.duty(i)
            servo_right.duty(i)
            sleep_ms(40)

        
    
    if y_value==511:
        for i in range(start_angle,120):
            servo_left.duty(i)
            servo_right.duty(i)
            sleep_ms(40)