from machine import Pin, PWM
import time

# Define the GPIO pins connected to the servos
left_servo_pin = 5   # Left servo pin
right_servo_pin = 18 # Right servo pin

# Set up the PWM for the servos
left_servo = PWM(Pin(left_servo_pin), freq=50)
right_servo = PWM(Pin(right_servo_pin), freq=50)

# Function to set the servo speed and direction
def set_servo_speed(servo, speed):
    # Convert speed (0 to 180) to duty cycle (500-2500 microseconds)
    duty = int((speed / 180) * (115 - 26) + 26)
    servo.duty(duty)

# Main loop
while True:
    # Forward motion: Increase speed from neutral (90) to max (180)
    for speed in range(90, 180):
        set_servo_speed(left_servo, speed)       # Rotate left servo forward
        set_servo_speed(right_servo, 180 - speed) # Rotate right servo forward in opposite direction
        time.sleep(0.02)                         # Delay for smooth motion
        print(f"Speed: {speed}")
        
    # Backward motion: Decrease speed from max (180) to neutral (90)
    for speed in range(180, 89, -1):
        set_servo_speed(left_servo, speed)       # Rotate left servo backward
        set_servo_speed(right_servo, 180 - speed) # Rotate right servo backward in opposite direction
        time.sleep(0.02)                         # Delay for smooth motion
        print(f"Speed: {speed}")