from  machine import Pin, ADC, PWM
from  time import sleep
import  utime

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))
    

button = Pin(14, Pin.IN)
button2 = Pin(13, Pin.IN)

servo = PWM(Pin(23))
servo2 = PWM(Pin(22))
servo3 = PWM(Pin(19))
servo4 = PWM(Pin(18))
servo5 = PWM(Pin(5))

servo.freq(50)
servo2.freq(50)
servo3.freq(50)
servo4.freq(50)
servo5.freq(50)

sw1 = ADC(Pin(35))
vx1 = ADC(Pin(32))
vy1 = ADC(Pin(33))

sw2 = ADC(Pin(25))
vx2 = ADC(Pin(26))
vy2 = ADC(Pin(27))


#RESOLUCION(EL RANGO QUE APARECE)
vx1.width(ADC.WIDTH_10BIT)
vx2.width(ADC.WIDTH_10BIT)
vy1.width(ADC.WIDTH_10BIT)
vy2.width(ADC.WIDTH_10BIT)


while True:
      sleep(0.1)
      Valorx1 = vx1.read()
      Valory1 = vy1.read()

      angulo1 = Map(Valorx1, 0, 1023, 0, 180)
      angulo2 = Map(Valory1, 0, 1023, 0, 180)

      duty = ((3.06084*angulo1**2 + 10278*angulo1 + 550000))
      convertir = duty/1000000
      mili = int ((convertir*1023)/20)
      servo.duty(mili)
      

      duty2 = ((3.06084*angulo2**2 + 10278*angulo2 + 550000))
      convertir2 = duty2/1000000
      mili2 = int ((convertir2*1023)/20)
      servo2.duty(mili2)
      print("H J1: ", Valorx1, " V J1: ", Valory1)

      Valorx2 = vx2.read()
      Valory2 = vy2.read()

      angulo3 = Map(Valorx2, 0, 1023, 0, 180)
      angulo4 = Map(Valory2, 0, 1023, 0, 180)

      duty3 = ((3.06084*angulo3**2 + 10278*angulo3 + 550000))
      convertir3 = duty3/1000000
      mili3 = int ((convertir3*1023)/20)
      servo3.duty(mili3)

      duty4 = ((3.06084*angulo4**2 + 10278*angulo4 + 550000))
      convertir4 = duty4/1000000
      mili4 = int ((convertir4*1023)/20)
      servo4.duty(mili4)
 
      print("H J2: ", Valorx2, " V J2: ", Valory2)

      if (button.value()):
         angulox = 90
         print(angulox)
         duty5 = ((3.06084*angulox**2 + 10278*angulox+ 550000))
         convertir5 = duty5/1000000
         mili5 = int ((convertir5*1023)/20)
         servo5.duty(mili5)

      print(button.value())  

      if (button2.value()):
         angulox = 180
         print(angulox)
         duty5 = ((3.06084*angulox**2 + 10278*angulox+ 550000))
         convertir5 = duty5/1000000
         mili5 = int ((convertir5*1023)/20)
         servo5.duty(mili5)