from machine import Pin, ADC, PWM
import utime

#declaring variables
Joystick_H= ADC(26)
Joystick_V= ADC(27)
Joystick_button=Pin(0,Pin.IN,Pin.PULL_UP)

R_button=Pin(1,Pin.IN,Pin.PULL_DOWN)
Y_button=Pin(2,Pin.IN,Pin.PULL_DOWN)
B_button=Pin(3,Pin.IN,Pin.PULL_DOWN)
G_button=Pin(4,Pin.IN,Pin.PULL_DOWN)

Servo1=PWM(Pin(22))
Servo2=PWM(Pin(21))
Servo3=PWM(Pin(20))
Servo4=PWM(Pin(19))

Servo1.freq(50)
Servo2.freq(50)
Servo3.freq(50)
Servo4.freq(50)

duty1=1500000
duty2=1500000
duty3=1500000
duty4=1500000

#initializing the servo
Servo1.duty_ns(1000000)
Servo2.duty_ns(1000000)
Servo3.duty_ns(1000000)
Servo4.duty_ns(1000000)

while True:
    Hor=int(Joystick_H.read_u16()*180/65535)
    Ver=int(Joystick_V.read_u16()*180/65535)

    B0_Value= Joystick_button.value()
    B1_Value=R_button.value()
    B2_Value=Y_button.value()
    B3_Value=B_button.value()
    B4_Value=G_button.value()
    
    if B0_Value==1:
        utime.sleep(0.5)
    else:
        Servo1.duty_ns(1500000)
        Servo2.duty_ns(1500000)
        Servo3.duty_ns(1500000)
        Servo4.duty_ns(1500000)
        duty1=1500000
        duty2=1500000
        utime.sleep(0.5)
        print("moving to initial position")

    
# Controlling Servo1 with Horizontal movement 
    if Hor<50 and duty1>500000 :
        Servo1.duty_ns(duty1)
        duty1-=10000
        print("posx: "+ str(Hor) + "and posy: " + str(Ver))
        print("duty1: " +str(duty1) + "and duty2: " + str(duty2))
        utime.sleep(0.2)

    elif Hor>140 and duty1<2500000 :
        Servo1.duty_ns(duty1)
        duty1+=10000
        print("posx: "+ str(Hor) + "and posy: " + str(Ver))
        print("duty1: " +str(duty1) + "and duty2: " + str(duty2))
        utime.sleep(0.2)
        
# Controlling Servo2 with Vertical movement 
    if Ver<50 and duty2>500000 :
        Servo2.duty_ns(duty2)
        duty2-=10000
        print("posx: "+ str(Hor) + "and posy: " + str(Ver))
        print("duty1: " +str(duty1) + "and duty2: " + str(duty2))
        utime.sleep(0.2)

    elif Ver>140 and duty2<2500000 :
        Servo2.duty_ns(duty2)
        duty2+=10000
        print("posx: "+ str(Hor) + "and posy: " + str(Ver))
        print("duty1: " +str(duty1) + "and duty2: " + str(duty2))
        utime.sleep(0.2)
        
# Controlling Servo3 with B1 and B2 movement 
    if B1_Value==1 and duty3>500000 :
        Servo3.duty_ns(duty3)
        duty3-=10000
        print("duty3: " +str(duty3) + "and duty4: " + str(duty4))
        utime.sleep(0.2)

    elif B2_Value==1 and duty1<2500000 :
        Servo3.duty_ns(duty3)
        duty3+=10000
        print("duty3: " +str(duty3) + "and duty4: " + str(duty4))
        utime.sleep(0.2)
        
# Controlling Servo4 with B3 and B4 movement 
    if B3_Value==1 and duty4>500000 :
        Servo4.duty_ns(duty4)
        duty4-=10000
        print("duty3: " +str(duty3) + "and duty4: " + str(duty4))
        utime.sleep(0.2)

    elif B4_Value==1 and duty4<2500000 :
        Servo4.duty_ns(duty4)
        duty4+=10000
        print("duty3: " +str(duty3) + "and duty4: " + str(duty4))
        utime.sleep(0.2)