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

#define PCA9685_ADDRESS  0x40
// called this way, it uses the default address 0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(PCA9685_ADDRESS);

#define SERVOMIN 150    // This is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX 600   // This is the 'maximum' pulse length count (out of 4096)
#define SERVO_FREQ 50  // Analog servos run at ~50 Hz updates

// Adjust these angles according to Servo360_PCA_AngleFInd code that I have provided
int leftAngle = 0;     // angel for motor to move left (If the motor move in right you can change the name to rightAngle)
int rightAngle = 180;  // angel for motor to move right (If the motor move in left you can change the name to leftAngle)
int stopAngle = 90;    // Angle when motor stops

// our servo # counter
uint8_t servonum = 0;
byte totalServos = 3;  // Define how many servos we are using here

// Define the time (in milliseconds 1 second = 1000 millisecond) for each servo to move in right direction
int rightMoveTime[13] = { 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000 };  // Wait for this millisecond when motor move right
int stopWaitTime[13] = { 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000 };   // Wait for this millisecond after motor has reached it's point
int leftMoveTime[13] = { 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000 };   // Wait for this millisecond when motor move left

void setup() {
  pwm.begin();  // Initialize PWM instance
  pwm.setOscillatorFrequency(27000000);
  pwm.setPWMFreq(SERVO_FREQ);  // Analog servos run at ~50 Hz updates

  // Stop all servos
  for (int servoNum = 0; servoNum < totalServos; servoNum++) {  // initially stop all servo motors
    pwm.setPWM(servoNum, 0, getAnglePulse(stopAngle));
  }
}

void loop() {
  // Run one servo at a time
  for (int currentServo = 0; currentServo < totalServos; currentServo++) {
    pwm.setPWM(currentServo, 0, getAnglePulse(rightAngle));  // First move servo to right side
    delay(rightMoveTime[currentServo]);                                    // move motor in right for this time defined in start
    pwm.setPWM(currentServo, 0, getAnglePulse(stopAngle));   // Stop servo at this point
    delay(stopWaitTime[currentServo]);                                    // Keep motor stopped at this position defined in start
    pwm.setPWM(currentServo, 0, getAnglePulse(leftAngle));   // Now move servo to left side back
    delay(leftMoveTime[currentServo]);                                     // move motor in left for this time defined in start
    pwm.setPWM(currentServo, 0, getAnglePulse(stopAngle));   // Stop This servo
    delay(50);                                               // wait for 50 milliseconds before moving next servo
  }
}
int getAnglePulse(int inpAngle) {
  int retPulse = map(inpAngle, leftAngle, rightAngle, SERVOMIN, SERVOMAX);
  return retPulse;
}