/* PCA9685ServoButtonsV3
  31/12/23
  Built on the foundations of PCA9685ServoButtonsV2
  This time the relay code has been added.

    

    This example uses 2 servos, one on Pin 0, the other on pin 12
    Relays are on pins 12 and 13

    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
   2 relay modules

   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
  pins 12-13 relays

*/

#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][5] = { //2 lines per servo (2 buttons)
  {8,0,45,12,0}, //pin 8, servo 0, position,relay,off
  {9,0,179,12,1}, //pin 9, servo 0, position,relay,on
  {10,12,33,13,0}, //pin 10,servo 12, position,relay,off
  {11,12,168,13,1} //pin 11, servo 12, position,relay,on
};
//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]);//move the servo
        digitalWrite(servoVariables[q][3],servoVariables[q][4]);//set the relay to appropriate value
        q=numButtons;//causes system to exit for loop and wait for debounce again    
      }
    }
  }
}



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

  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
    pinMode(servoVariables[q][3], OUTPUT);//set the pins used for relays servoVariables array
  }

}

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