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

// #################  Just for Testing #######################
#include "Streaming.h"
#include "ESP32Servo.h"
Servo myservo;
// ###########################################################

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40);  // PCA9685 default I2C address

#define SERVOMIN 160  //MG995 153.6 round up to 160
#define SERVOMAX 540 //MG995 532.5 round up to 540

ezButton button1(18); // create ezButton object that attach to pin 7;
ezButton button2(19); // create ezButton object that attach to pin 7;

int Servo_Pin = 0;     
int angle;

void setup() {

// #################  Just for Testing #######################
   Serial.begin(115200);
   myservo.attach(15);
// ###########################################################

  button1.setDebounceTime(50); // set debounce time to 50 milliseconds
  button2.setDebounceTime(50); // set debounce time to 50 milliseconds
  pwm.begin();         
  pwm.setPWMFreq(50);   
  delay(1000);

}

void loop() {

  button1.loop(); // MUST call the loop() function first

  if (button1.isPressed()) {
  angle = 0;

// #################  Just for Testing #######################
   Serial << "Button 1 :\t";
   myservo.write(angle);
// ###########################################################
   
   ServoGoto(angle);

  }

  button2.loop(); // MUST call the loop() function first

  if (button2.isPressed()) {
  angle = 180;

// #################  Just for Testing #######################
   Serial << "Button 2 :\t";
   myservo.write(angle);
// ##########################################################  

   ServoGoto(angle);

  }

}

void ServoGoto(int angle){
  angle = map(angle, 0, 180, SERVOMIN, SERVOMAX);
  pwm.setPWM(Servo_Pin, 0, angle);

// #################  Just for Testing #######################
   Serial << angle << "\n";
// ###########################################################

  delay(50);
}