import time
from machine import Pin, PWM, I2C
from servos import Servos

sda = Pin(20)
scl = Pin(21)
id = 0

i2c = I2C(id=id, sda=sda, scl=scl)

print(i2c.scan())
servo = Servos(i2c=i2c, address=0x40)

print("Set all servos to zero")
for index in range(0,16):
    servo.position(index=index, degrees=0)

time.sleep(0.5)

print("Slowly rotate all servos")
for time_step in range(0,180):
  for index in range(0,16):
    servo.position(index=index, degrees=time_step)
  time.sleep(0.03)

time.sleep(0.5)
print("Set back to zero")
for index in range(0,16):
    servo.position(index=index, degrees=0)

time.sleep(0.5)
print("Rotate servos in wave")
for time_step in range(0,180):
  for index in range(0,16):
    servo.position(index=index, degrees=time_step+20*index)
  time.sleep(0.03)