from machine import Pin, I2C
import time
from pca9685 import PCA9685

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)