from machine import Pin, ADC
from time import sleep
from servo import Servo


base_servo = Servo(25)
shoulder_servo = Servo(26)
elbow_servo = Servo(27)

joystick_x = ADC(Pin(34))  
joystick_y = ADC(Pin(35)) 
button = Pin(32, Pin.IN, Pin.PULL_UP)  

current_servo = base_servo

def move_servo():
    x_pos = joystick_x.read()  # Read X-axis
    y_pos = joystick_y.read()  # Read Y-axis

    # Map joystick values to angles
    angle_x = int((x_pos / 4095) * 180)
    angle_y = int((y_pos / 4095) * 180)

    current_servo.move(180-angle_x)

def switch_servo():
    global current_servo
    if button.value() == 0:  # Button pressed
        if current_servo == base_servo:
            current_servo = shoulder_servo
        elif current_servo == shoulder_servo:
            current_servo = elbow_servo
        else:
            current_servo = base_servo
        sleep(0.2)  # Debounce delay

# Main loop
while True:
    switch_servo()
    move_servo()
    sleep(0.05)