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)
led = Pin(12, Pin.OUT)
led2 = Pin(14, Pin.OUT)
led.on()

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(26))
vx1 = ADC(Pin(33))
vy1 = ADC(Pin(25))

sw2 = ADC(Pin(27))
vx2 = ADC(Pin(32))
vy2 = ADC(Pin(35))


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

angulo1 = 90
angulo2 = 90
angulo3 = 90
angulo4 = 90
angulox = 90
AUX = 0

while True:
      sleep(0.1)
      Valorx1 = vx1.read()
      Valory1 = vy1.read()
      
      if Valorx1 >=900:
         angulo1 = angulo1 + 6
         if angulo1 < 180:
             print(angulo1)
             duty = ((3.06084*angulo1**2 + 10278*angulo1 + 550000))
             convertir = duty/1000000
             mili = int ((convertir*1023)/20)
             servo.duty(mili)
         else:
             angulo1 = 180  
             print(angulo1)
             duty = ((3.06084*angulo1**2 + 10278*angulo1 + 550000))
             convertir = duty/1000000
             mili = int ((convertir*1023)/20)
             servo.duty(mili)
         
      if Valorx1 <=300:
         angulo1 = angulo1 - 6
         if angulo1 > 0:
             print(angulo1)
             duty = ((3.06084*angulo1**2 + 10278*angulo1 + 550000))
             convertir = duty/1000000
             mili = int ((convertir*1023)/20)
             servo.duty(mili)
         else:
             angulo1 = 0  
             print(angulo1)
             duty = ((3.06084*angulo1**2 + 10278*angulo1 + 550000))
             convertir = duty/1000000
             mili = int ((convertir*1023)/20)
             servo.duty(mili)

      if Valory1 >=900:
         angulo2 = angulo2 + 6
         if angulo2 < 180:
             print(angulo2)
             duty2 = ((3.06084*angulo2**2 + 10278*angulo2 + 550000))
             convertir2 = duty2/1000000
             mili2 = int ((convertir2*1023)/20)
             servo2.duty(mili2)
         else:
             angulo2 = 180  
             print(angulo2)
             duty2 = ((3.06084*angulo2**2 + 10278*angulo2 + 550000))
             convertir2 = duty2/1000000
             mili2 = int ((convertir2*1023)/20)
             servo2.duty(mili2)

      if Valory1 <=300:
         angulo2 = angulo2 - 6
         if angulo2 > 0:
             print(angulo2)
             duty2 = ((3.06084*angulo2**2 + 10278*angulo2 + 550000))
             convertir2 = duty2/1000000
             mili2 = int ((convertir2*1023)/20)
             servo2.duty(mili2)
         else:
             angulo2 = 0  
             print(angulo2)
             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()

      if AUX == 0:
         if Valorx2 >=900:
             angulo3 = angulo3 + 6
             if angulo3 < 180:
                 print(angulo3)
                 duty3 = ((3.06084*angulo3**2 + 10278*angulo3 + 550000))
                 convertir3 = duty3/1000000
                 mili3 = int ((convertir3*1023)/20)
                 servo3.duty(mili3)
             else:
                 angulo3 = 180  
                 print(angulo3)
                 duty3 = ((3.06084*angulo3**2 + 10278*angulo3 + 550000))
                 convertir3 = duty3/1000000
                 mili3 = int ((convertir3*1023)/20)
                 servo3.duty(mili3)
             
         if Valorx2 <=300:
             angulo3 = angulo3 - 6
             if angulo3 > 0:
                 print(angulo3)
                 duty3 = ((3.06084*angulo3**2 + 10278*angulo3 + 550000))
                 convertir3 = duty3/1000000
                 mili3 = int ((convertir3*1023)/20)
                 servo3.duty(mili3)
             else:
                 angulo3 = 0  
                 print(angulo3)
                 duty3 = ((3.06084*angulo3**2 + 10278*angulo3 + 550000))
                 convertir3 = duty3/1000000
                 mili3 = int ((convertir3*1023)/20)
                 servo3.duty(mili3)
         if Valory2 >=900:
             angulo4 = angulo4 + 6
             if angulo4 < 180:
                 print(angulo4)
                 duty4 = ((3.06084*angulo4**2 + 10278*angulo4 + 550000))
                 convertir4 = duty4/1000000
                 mili4 = int ((convertir4*1023)/20)
                 servo4.duty(mili4)
             else:
                 angulo4 = 180  
                 print(angulo4)
                 duty4 = ((3.06084*angulo4**2 + 10278*angulo4 + 550000))
                 convertir4 = duty4/1000000
                 mili4 = int ((convertir4*1023)/20)
                 servo4.duty(mili4)
         if Valory2 <=300:
             angulo4 = angulo4 - 6
             if angulo4 > 0:
                 print(angulo4)
                 duty4 = ((3.06084*angulo4**2 + 10278*angulo4 + 550000))
                 convertir4 = duty4/1000000
                 mili4 = int ((convertir4*1023)/20)
                 servo4.duty(mili4)
             else:
                 angulo4 = 0  
                 print(angulo4)
                 duty4 = ((3.06084*angulo4**2 + 10278*angulo4 + 550000))
                 convertir4 = duty4/1000000
                 mili4 = int ((convertir4*1023)/20)
                 servo4.duty(mili4)

      if AUX ==1:
         if Valorx2 >=900:
             angulox = angulox + 6
             if angulox < 180:
                 print(angulox)
                 duty5 = ((3.06084*angulox**2 + 10278*angulox + 550000))
                 convertir5 = duty5/1000000
                 mili5 = int ((convertir5*1023)/20)
                 servo5.duty(mili5)
             else:
                 angulox = 180  
                 print(angulox)
                 duty5 = ((3.06084*angulox**2 + 10278*angulox + 550000))
                 convertir5 = duty5/1000000
                 mili5 = int ((convertir5*1023)/20)
                 servo5.duty(mili5)    
         if Valorx2 <=300:
             angulox = angulox - 6
             if angulox > 0:
                 print(angulox)
                 duty5 = ((3.06084*angulox**2 + 10278*angulox + 550000))
                 convertir5 = duty5/1000000
                 mili5 = int ((convertir5*1023)/20)
                 servo5.duty(mili5)
             else:
                 angulox = 0  
                 print(angulox)
                 duty5 = ((3.06084*angulox**2 + 10278*angulox + 550000))
                 convertir5 = duty5/1000000
                 mili5 = int ((convertir5*1023)/20)
                 servo5.duty(mili5)

      print("H J2: ", Valorx2, " V J2: ", Valory2)

      print("AUX ", AUX)

      if (button.value()):
         AUX = 1   
         led2.on()

      if (button2.value()):
         AUX = 0
         led2.off()