import machine
import time
import pca9685
import servo

sdaPIN=machine.Pin(13)
sclPIN=machine.Pin(14)
i2c=machine.I2C(0,sda=sdaPIN, scl=sclPIN)


# Initialize the PCA9685 using the default address (0x40).
pwm = pca9685.PCA9685(address=0x5f, i2c = i2c)

# Set the PWM frequency to 60Hz, good for servos.
pwm.freq(50)

# test servos
# pan serv0
servo1 = servo.Servos(address=0x5f, i2c = i2c)
#tilt servo
servo2 = servo.Servos(address=0x5f, i2c = i2c)

for n in range(5):
    servo1.position(index=0, degrees=90)
    deg_rotate = 90
    for k in range(deg_rotate):
        ang = 90 + k
        servo1.position(index=0, degrees=ang)
        time.sleep(.01)
    angend = 90 + k
    for k in range(2*deg_rotate):
        ang = angend - k
        servo1.position(index=0, degrees=ang)
        time.sleep(.01)
    angend = angend - k
    for k in range(deg_rotate):
        ang = angend + k
        servo1.position(index=0, degrees=ang)
        time.sleep(.01)

    #tilt servo
    servo2.position(index=1, degrees=90)
    deg_rotate = 45
    for k in range(deg_rotate):
        ang = 90 + k
        servo1.position(index=1, degrees=ang)
        time.sleep(.01)
    angend = 90 + k
    for k in range(deg_rotate):
        ang = angend - k
        servo1.position(index=1, degrees=ang)
        time.sleep(.01)