#include <Wire.h>  //Include wire library for I2C
#include<Adafruit_PWMServoDriver.h>

Adafruit_PWMServoDriver pca9685 = Adafruit_PWMServoDriver(0x40); //Creat object to represent PCA9685 at defauult I2C address (modules & objects)
//                      name                           (address)
#define SERVOMIN 80 //Min value
#define SERVOMAX 600 //Max value
//This deifines the max/min # of "ticks" for the servo motors which determins the pulse with, range 0-4096

#define SER0 0  //Servo Motor 0 on connector 0
#define SER1 1  //Servo Motor 0 on connector 12

int pwm0;
int pwm1;
// Variables for servo motor position
int posDegree;

void setup(){
  Serial.begin(115200);
  Serial.println("PCA9685 Servo Test");
  pca9685.begin();  //Initializes the PCA9685
  pca9685.setPWMFreq(50); // Sets PWM Frequency to 50Hz
}

void loop(){
  
  // Move Servo 0 from 0 to 180
  for (posDegree = 0; posDegree <=180; posDegree++){
    pwm0 = map(posDegree,0,180, SERVOMIN,SERVOMAX);  //determin PWM pulse width
    pca9685.setPWM(SER0,0,pwm0); //write to PCA9685  
    Serial.print("Motor 0 = ");
    Serial.println(posDegree);
    delay(30);
  }
  // Move Servo 1 from 180 to 0
  for (posDegree = 180; posDegree >=0; posDegree--){
    pwm1 = map(posDegree,0,180, SERVOMIN,SERVOMAX);  //determin PWM pulse width
    pca9685.setPWM(SER1,0,pwm1); //write to PCA9685  
    Serial.print("Motor 1 = ");
    Serial.println(posDegree);
    delay(30);
  }
  
  // Move Servo 0 from 0 to 180
  for (posDegree = 180; posDegree >=0; posDegree--){
    pwm0 = map(posDegree,0,180, SERVOMIN,SERVOMAX);  //determin PWM pulse width
    pca9685.setPWM(SER0,0,pwm0); //write to PCA9685  
    Serial.print("Motor 0 = ");
    Serial.println(posDegree);
    delay(30);
  }
  // Move Servo 1 from 0 to 180
  for (posDegree = 0; posDegree <=180; posDegree++){
    pwm1 = map(posDegree,0,180, SERVOMIN,SERVOMAX);  //determin PWM pulse width
    pca9685.setPWM(SER1,0,pwm1); //write to PCA9685  
    Serial.print("Motor 1 = ");
    Serial.println(posDegree);
    delay(30);
  }
}