/* PCA9685ServoButtonsV2
  31/12/23
  Built on the foundations of PCA9685ServoButtonsV1 but with all servo pin and settings data stored in an array.
  This is to make it easier to add more servos/buttons

    NON relay

    This example shows how to control the servos using buttons.
    This code DOES NOT have any relay code so for swithcing points
    you will need to use the point blades

    This example uses 2 servos, one on Pin 0, the other on pin 12

    adding more buttons/servos is just a matter of extending the code.

   Code is written as a state machine without delay() so that other code (without delay()
   can be added if needed.

   PCA9685 board
   2 servos
   4 buttons
   4 x 4.5k OHM resistors for Pull Down on buttons

   SDA/SCL connections for PCA9685

  Arduino UNO: A4 (SDA), A5 (SCL)
  Arduino Mega 2560: 20 (SDA), 21 (SCL)
  ESP32: 21(SDA), 22 (SCL)

  pins 8 - 11 for buttons with 4.7K pull down resistors
  
*/

#include "Wire.h"
#include "Adafruit_PWMServoDriver.h"
//PCA 9685 settings
// called this way, it uses the default address 0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
// you can also call it with a different address you want
//Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x41);
// you can also call it with a different address and I2C interface
//Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40, Wire);

#define USMIN  600 // This is the rounded 'minimum' microsecond length based on the minimum pulse of 150
#define USMAX  2400 // This is the rounded 'maximum' microsecond length based on the maximum pulse of 600
#define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates
//end pca9685 settings

//position in the following array is the values you would get from the PCA9685 Servo setter project
//see https://www.digitaltown.co.uk/projectPCA9685ServoSetter.php
# define numButtons 4 //number of buttons controlling servos
int servoVariables[numButtons][3] = { //2 lines per servo (2 buttons)
  {8,0,45}, //pin 8, servo 0, position
  {9,0,179}, //pin 9, servo 0, position
  {10,12,33}, //pin 10,servo 12, position
  {11,12,168} //pin 11, servo 12, position
};
//System variables
unsigned long currentMillis; //will stire the current time in from millis()
unsigned long buttonTimer;//stores the next value in millis for debounce
int buttonDebounce = 200;//smaller vale makes buttons more sensitive, easier to get double press

//Servo movement function
void setServoPos(int servo, int pos) {
  //This first bit of code makes sure we are not trying to set the servo outside of limits
  int sendPos;
  if (pos > 179) {
    pos = 179;
  }
  if (pos < 0) {
    pos = 0;
  }
  sendPos = USMIN + ((USMAX - USMIN) / 180 * pos);
  if (servo > -1 && servo < 16) { //only try to move valid servo addresses
    pwm.writeMicroseconds(servo, sendPos);
  }
}

//deals with button presses
//This works but would become very complicated and hard to read with 16 servos
void buttonState() {
  int q;
  if (currentMillis - buttonTimer >= buttonDebounce) { //are buttons debounced
    buttonTimer = currentMillis;//reset debounce timer
    for(q=0;q < numButtons;q++){
      if(digitalRead(servoVariables[q][0]) > 0){
        setServoPos(servoVariables[q][1], servoVariables[q][2]);
        q=numButtons;//causes system to exit for loop and wait for debounce again    
      }
    }
  }
}



void setup() {
  Serial.begin(9600);
  Serial.println("PCA9685ServoSetterV2");

  pwm.begin();
  pwm.setOscillatorFrequency(27000000);
  pwm.setPWMFreq(SERVO_FREQ);  // Analog servos run at ~50 Hz updates

  delay(10);//let the above values take effect

  for(int q=0;q < numButtons;q++){
    pinMode(servoVariables[q][0], INPUT);//set the pins used for buttons in the servoVariables array
  }

}

void loop() {
  currentMillis = millis();//get the current millis since board started for timing
  buttonState();//deals with button presses
}