from machine import Pin, ADC, PWM
import utime

# Joystick
xAxis = ADC(Pin(27))
yAxis = ADC(Pin(26))
button = Pin(16, Pin.IN, Pin.PULL_UP)
# Servo1
servo = PWM(Pin(15))
servo.freq(50)
# Servo2
servo2 = PWM(Pin(14))
servo2.freq(50)

new_xValue = 0
servoValue = 0
servo2Value = 0
angle_actuel = 4750
angle_actuel2 = 4750

while True:
  # Joystick
  xValue = xAxis.read_u16()
  yValue = yAxis.read_u16()
  buttonValue = button.value()
  #print(str(xValue) + ", " + str(yValue) + ", " + str(buttonValue))

  # Servo1
  servo.duty_u16(angle_actuel)
  utime.sleep(0.02)
  if xValue > 32759:
    if angle_actuel < 8000:
      angle_actuel += 40
    print('Droite')
    #print('Angle actuel: ' + str(angle_actuel))

  if xValue < 32759:
    print('Gauche')
    if angle_actuel > 1500:
      angle_actuel -= 40

  if buttonValue == 0:
    angle_actuel = 4750
    angle_actuel2 = 4750

  # Servo2
  servo2.duty_u16(angle_actuel2)
  #utime.sleep(0.02)
  if yValue > 32759:
    print('Bas')
    if angle_actuel2 < 8000:
      angle_actuel2 += 40
  
  servo2.duty_u16(angle_actuel2)
  #utime.sleep(0.02)
  if yValue < 32759:
    print('Haut')
    if angle_actuel2 > 1500:
      angle_actuel2 -= 40