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

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

#define MIN_PULSE_WIDTH       650
#define MAX_PULSE_WIDTH       2350
#define DEFAULT_PULSE_WIDTH   1700
#define FREQUENCY             50
#include <Servo.h>
int8_t servonum = 0;
int input;

void setup() {
  Serial.begin(9600);
  Serial.println("16 channel Servo test!");
  pwm.begin();
  pwm.setPWMFreq(FREQUENCY);
}
int pulseWidth(int angle)
{
  int pulse_wide, analog_value;
  pulse_wide   = map(angle, 0, 180, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
  analog_value = int(float(pulse_wide) / 1000000 * FREQUENCY * 4096);
  Serial.println( pulse_wide);
  return analog_value;
}

void loop() {
  
 pwm.setPWM(0, 0, pulseWidth(90));
 pwm.setPWM(1, 0, pulseWidth(90));
 pwm.setPWM(2, 0, pulseWidth(90));
 pwm.setPWM(3, 0, pulseWidth(90));
 pwm.setPWM(4, 0, pulseWidth(90));
 pwm.setPWM(5, 0, pulseWidth(90));
 pwm.setPWM(6, 0, pulseWidth(90));
 
 delay(200);// waits for the servo to get there

 pwm.setPWM(0, 0, pulseWidth(0));
 pwm.setPWM(1, 0, pulseWidth(0));
 pwm.setPWM(2, 0, pulseWidth(180));
 pwm.setPWM(3, 0, pulseWidth(180));
 pwm.setPWM(4, 0, pulseWidth(45));
 pwm.setPWM(5, 0, pulseWidth(45));
 pwm.setPWM(6, 0, pulseWidth(0));

 delay(200);// waits for the servo to get there

 pwm.setPWM(0, 0, pulseWidth(45));
 pwm.setPWM(1, 0, pulseWidth(45));
 pwm.setPWM(2, 0, pulseWidth(180));
 pwm.setPWM(3, 0, pulseWidth(180));
 pwm.setPWM(4, 0, pulseWidth(0));
 pwm.setPWM(5, 0, pulseWidth(0));
 pwm.setPWM(6, 0, pulseWidth(0));

 delay(200);// waits for the servo to get there

 pwm.setPWM(0, 0, pulseWidth(90));
 pwm.setPWM(1, 0, pulseWidth(90));
 pwm.setPWM(2, 0, pulseWidth(90));
 pwm.setPWM(3, 0, pulseWidth(90));
 pwm.setPWM(4, 0, pulseWidth(90));
 pwm.setPWM(5, 0, pulseWidth(90));
 pwm.setPWM(6, 0, pulseWidth(90));
 
 delay(200);// waits for the servo to get there
}