// Include Wire Library for I2C
#include <Wire.h>
#include "ezButton.h"
#include "Adafruit_PWMServoDriver.h"

// Creat object to represent PCA9685 at default I2C address
Adafruit_PWMServoDriver pca9685 = Adafruit_PWMServoDriver(0x40);

// Define maximum and minimum number of "ticks" for the servo motors
// Range from 0 to 4095
// This determines the pulse width

// Define servo motor connections (expand as required)
// Read the manual and check your specific installation!
// This is just a part of the example but probably has to be adopted:
#define SERVOMIN  125  // Minimum value - previously 80
#define SERVOMAX  575  // Maximum value - previously 1024
#define SER0  0   //Servo Motor 0 on connector 0
#define SER1  4  //Servo Motor 1 on connector 12

// Variables for Servo Motor positions (expand as required)
int pwm0;
int pwm1;

//Servo myservo;  // create servo object to control a servo
//Servo myservo1;
// 16 servo objects can be created on the ESP32
ezButton button1(18); // create ezButton object that attach to pin 7;
ezButton button2(19); // create ezButton object that attach to pin 7;

int pos = 0;    // variable to store the servo position


//from button sketch
int angle = 0; // the current angle of servo motor
int angle2 = 0; // the current angle of servo motor


void setup() {


  
  // Serial monitor setup
  Serial.begin(115200);

  button1.setDebounceTime(50); // set debounce time to 50 milliseconds
  button2.setDebounceTime(50); // set debounce time to 50 milliseconds

//  myservo.write(angle);
//  myservo1.write(angle);

  // Print to monitor
  Serial.println("PCA9685 Servo Test");

  // Initialize PCA9685
  pca9685.begin();

  // Set PWM Frequency to 50Hz
  pca9685.setPWMFreq(50);

}



void loop() {

  button1.loop(); // MUST call the loop() function first
  if (button1.isPressed()) {
    // change angle of servo motor
    if (angle == 0)
      angle = 90;
    else if (angle == 90)
      angle = 0;
      // control servo motor arccoding to the angle
    Serial.print("The button is pressed => rotate servo to ");
    Serial.print(angle);
    Serial.println("°");
 
  // Move Motor 1 from 0 to 180 degrees
  for (int posDegrees = 0; posDegrees <= 180; posDegrees++) {
     if (posDegrees % 10 == 0) Serial.println(posDegrees);
     pwm0 = map(posDegrees, 0, 180, SERVOMIN, SERVOMAX);
     pca9685.setPWM(SER0, 0, pwm0);
  }
  while (button1.isPressed())  button1.loop(); 
  }
}