from machine import PWM, Pin
from time import sleep

# declare two servos on GP0 and GP1
left_servo = PWM(Pin(0))
right_servo = PWM(Pin(1))

# set the servo frequency to 50Hz
left_servo.freq(50)
right_servo.freq(50)

min_duty = 2000   # corresponds to 0 degrees
min_angle = -90
max_duty = 8000   # corresponds to 180 degrees
max_angle = 90

def linear_interpolate(x, in_min, in_max, out_min, out_max):
    return out_min + (x - in_min) * (out_max - out_min) / (in_max - in_min)

def set_servo_angle(servo, angle):
    # Clamp angle to the defined range
    angle = max(min_angle, min(max_angle, angle))

    # Interpolate to get duty cycle
    duty = int(linear_interpolate(angle, min_angle, max_angle, min_duty, max_duty))
    print(f"Angle: {angle}°, Duty: {duty}")
    servo.duty_u16(duty)


# test some single values
interval = 2
set_servo_angle(left_servo, -90)
set_servo_angle(right_servo, 90)
sleep(interval)
set_servo_angle(left_servo, 90)
set_servo_angle(right_servo, -90)
sleep(interval)

while True:
    for duty in range(min_duty, max_duty, 256):  # Fade in
        left_servo.duty_u16(duty)
        right_servo.duty_u16(duty)
        print(duty)
        sleep(0.1)
    for duty in range(max_duty, min_duty, -256):  # Fade out
        left_servo.duty_u16(duty)
        right_servo.duty_u16(duty)
        print(duty)
        sleep(0.1)