from machine import Pin, I2C
import time
from servo import Servos

# Initialize I2C
i2c = I2C(0, sda=Pin(16), scl=Pin(17))

# Initialize Driver
driver = Servos(i2c)

# Move Servo on Channel 0 to 90 degrees (center)
driver.position(0, degrees=90)
driver.position(1, degrees=90)
time.sleep(1)

# Basic test sweep
while True:
    driver.position(0, degrees=0)
    driver.position(1, degrees=45)
    time.sleep(1)
    driver.position(0, degrees=180)
    driver.position(1, degrees=135)
    time.sleep(1)
"""
delay = None
i = None

MIN_DUTY = int(4096 * 500 / 20000)
MAX_DUTY = int(4096 * 2500 / 20000)
SERVO_SPAN = MAX_DUTY - MIN_DUTY

def angleToDutyCycle(angle):
	dutyCycle = int(MIN_DUTY + (angle * SERVO_SPAN / 180))
	dutyCycle = min(MAX_DUTY, max(MIN_DUTY, dutyCycle))
	return dutyCycle

i2c = I2C(0, sda=Pin(16), scl=Pin(17))  # Correct I2C pins for RP2040
pca = PCA9685(i2c)

pca.frequency = 50
delay = 10

# Set the PWM duty cycle for channel zero to 50%. duty_cycle is 16 bits to match other PWM objects
# but the PCA9685 will only actually give 12 bits of resolution.
while True:
  for i in range(0, 181):
    pca.channels[0].duty_cycle = i * 65
    pca.channels[1].duty_cycle = (180-i) * 65
    time.sleep_ms(delay)
  for i in range(180, 0, -1):
    pca.channels[0].duty_cycle = i * 65
    pca.channels[1].duty_cycle = (180-i) * 65
    time.sleep_ms(delay)
"""