#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 50    // This is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX 550   // 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 = 0;     // Angle when motor stops

// our servo # counter
uint8_t servonum = 0;
byte totalServos = 1;  // 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
    for (int i = rightAngle; i >= 0; i--) {
      pwm.setPWM(servoNum, 0, getAnglePulse(i));  // Stop This servo
      delay(5);                                       // this will control motor movement
    }
  }
}

void loop() {
  // Run one servo at a time
  for (int currentServo = 0; currentServo < totalServos; currentServo++) {
    int tempAng = 0;
    for (int i = rightAngle; i >= 0; i--) {
      pwm.setPWM(currentServo, 0, getAnglePulse(i));  // First move servo to right side
      delay(5);                                       // this will control motor movement
    }
    delay(rightMoveTime[currentServo]);  // move motor in right for this time defined in start
    for (int i = 0; i < rightAngle; i++) {
      pwm.setPWM(currentServo, 0, getAnglePulse(i));  // Stop This servo
      delay(5);                                       // this will control motor movement
    }
    delay(stopWaitTime[currentServo]);  // Stop motor for few seconds before moving to next
  }
}
int getAnglePulse(int inpAngle) {
  int retPulse = map(inpAngle, 0, 180, SERVOMIN, SERVOMAX);
  return retPulse;
}