import time
from adafruit_servokit import ServoKit

# This is set to zero for the max range
OFFSET = -10

MIN = 500 + OFFSET
MAX = 2500 - OFFSET
CHANNELS = 16
RANGE = 180
INIT = 0
FREQUENCY = 50

# Setup the kit
kit = ServoKit(channels=CHANNELS, frequency=FREQUENCY)

for i in range(CHANNELS):
    kit.servo[i].set_pulse_width_range(MIN, MAX)
    kit.servo[i].actuation_range = RANGE
    kit.servo[i].angle = INIT

time.sleep(2)

# Testing the accuracy of the lib
NUM = 8
OFFSET = 5

# Set the angle
def sa(angle=0, sleep=1.5):
    print(f"Set angle of pin {NUM+1} to {angle}.")
    kit.servo[NUM].angle = angle + OFFSET
    time.sleep(0.01)
    kit.servo[NUM].angle = angle
    kit.servo[NUM].angle = angle - OFFSET
    time.sleep(0.01)
    kit.servo[NUM].angle = angle
    time.sleep(sleep)

sa(80)
exit()

#for i in range(180):
    #sa(i, 0)
    #time.sleep(0.1)


# Run the actual testing
sa(10)
sa(90)
sa(30)
sa(170)
sa(60)
sa(150)
sa(90, 0)