from machine import Pin, PWM
import time

# Set up the PWM for each servo at 50Hz
servo_pin_1 = Pin(4, Pin.OUT)   # Servo 1 connected to D2
servo_pin_2 = Pin(2, Pin.OUT)  # Servo 2 connected to D15
servo_pin_3 = Pin(15, Pin.OUT)   # Servo 3 connected to D4

# Set PWM frequency to 50Hz (standard for servos)
pwm1 = PWM(servo_pin_1, freq=50)
pwm2 = PWM(servo_pin_2, freq=50)
pwm3 = PWM(servo_pin_3, freq=50)

# Set a neutral (0 degree) duty cycle for the servos (this may vary depending on your servo)
def move_servo(pwm, angle):
    # Calculate duty cycle based on angle
    # Assuming 0 degree -> 40 (1ms pulse), 180 degree -> 115 (2ms pulse)
    duty = int(40 + (angle / 180) * (115 - 40))  # Scale angle to 40-115 range
    pwm.duty(duty)

# Function to move the robot arm
def robot_arm_move():
    # Move servo 1 to 90 degrees, 2 to 45 degrees, 3 to 180 degrees
    move_servo(pwm1, 90)
    move_servo(pwm2, 45)
    move_servo(pwm3, 180)
    time.sleep(1)
    
    # Move servo 1 to 0 degrees, 2 to 90 degrees, 3 to 90 degrees
    move_servo(pwm1, 0)
    move_servo(pwm2, 90)
    move_servo(pwm3, 90)
    time.sleep(1)
    
    # Move servo 1 to 180 degrees, 2 to 135 degrees, 3 to 45 degrees
    move_servo(pwm1, 180)
    move_servo(pwm2, 135)
    move_servo(pwm3, 45)
    time.sleep(1)

# Loop to repeat the movement
while True:
    robot_arm_move()