from machine import PWM,Pin
from time import sleep
from servo import Servo
from joystick import JoyStick

# Initialize the servo motors
base_servo = Servo(22)
shoulder_servo = Servo(21)
elbow_servo = Servo(15)
myJoyStick = JoyStick(34,35,32)

def pick_and_place():
    #Move to pick position
    base_servo.move(90)
    shoulder_servo.move(45)
    elbow_servo.move(30)
    sleep(1)

    #Move to place position
    elbow_servo.move(90)
    base_servo.move(180)
    sleep(1)

    #Reset to start position
    shoulder_servo.move(90)
    elbow_servo.move(30)
    sleep(1)


print("Hello, ESP32!")

while True:
    pick_and_place()
    # vertical,horizantal = myJoyStick.get_joystick_readings()

    # button = myJoyStick.get_button_state()
    # base_servo.move(vertical)
    # # print('Vertical angle',vertical)
    # shoulder_servo.move(horizantal)
    # # print('Horizontal angle ',horizantal)
    # # print('Button State',button)
    # if button == JoyStick.BUTTON_PRESSED:
    #     elbow_servo.rotate(50)
    # sleep(0.1)