#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

int controlPins[12] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11};
int pos = 0;

Adafruit_PWMServoDriver board1 = Adafruit_PWMServoDriver(0x40);

#define SERVOMIN  125 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX  575 // this is the 'maximum' pulse length count (out of 4096)
int servoNumber = 0;

void setup() {
  Serial.begin(9600);
  Serial.println("32 channel Servo test!");

  board1.begin();
  board1.setPWMFreq(60);
  //yield();
}

void loop() {
  for (int i = 0; i < 12; i++) {
    loopMotor(controlPins[i]);
    delay(1000);
  }
}

int angleToPulse(int ang) {
  int pulse = map(ang, 0, 180, SERVOMIN, SERVOMAX); // map angle of 0 to 180 to Servo min and Servo max
  Serial.print("Angle: "); Serial.print(ang);
  Serial.print(" pulse: "); Serial.println(pulse);
  return pulse;
}


void loopMotor(int motorPinNumber) {
  for (pos = 0; pos <= 180; pos += 1) {
    board1.setPWM(motorPinNumber, 0, angleToPulse(pos) );
    delay(15);
  }
  for (pos = 180; pos >= 0; pos -= 1) {
    board1.setPWM(motorPinNumber, 0, angleToPulse(pos) );
    delay(15);
  }
}