from machine import ADC, Pin, PWM
import time

#X1=ADC(Pin())
Y1=ADC(Pin(26))
X2=ADC(Pin(27))
Y2=ADC(Pin(28))

servo1=PWM(Pin(18))
servo1.freq(50)
servo2=PWM(Pin(19))
servo2.freq(50)
servo3=PWM(Pin(20))
servo3.freq(50)

DireccionStep=Pin(17,Pin.OUT)
PasoStep=Pin(16,Pin.OUT)



def Map(x, in_min, in_max, out_min, out_max):
    return int((x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min)


while True:
  #Giro=X1.read_u16()
  Inclinacion=Y1.read_u16()
  Levantar=Y2.read_u16()
  Tomar=X2.read_u16()
  
  #ValorGiro = Map(Giro, 0, 65535, 500000, 2500000)
  ValorInclinacion = Map(Inclinacion, 0, 65535, 550000, 2450000)
  ValorLevantar = Map(Levantar, 0, 65535, 550000, 2450000)
  ValorTomar = Map(Tomar, 0, 65535, 550000, 2450000)

  servo1.duty_ns(ValorInclinacion)
  servo2.duty_ns(ValorLevantar)
  servo3.duty_ns(ValorTomar)