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

# Initialize the servo motors
base_servo = Servo(25)
shoulder_servo = Servo(26)
elbow_servo = Servo(27)

# Initialize joystick and button
joystick_x = ADC(Pin(34))  # Joystick X-axis
joystick_y = ADC(Pin(35))  # Joystick Y-axis
button = Pin(32, Pin.IN, Pin.PULL_UP)  # Button to switch servos

# Start with base servo
current_servo = base_servo

# Function to move current servo based on joystick input
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)

    # Control the selected servo using x or y
    # if current_servo == base_servo:
    #     current_servo.move(angle_x)
    # elif current_servo == shoulder_servo:
    #     current_servo.move(angle_y)
    # elif current_servo == elbow_servo:
    #     current_servo.move(angle_y)

# Function to switch between servos
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)