/* PCA9685ServoButtonsV1
    31/12/23
    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

//buttons...will all need pull down resistors
const int servo0Left = 8;
const int servo0Right = 9;
const int servo12Left = 10;
const int servo12Right = 11;

// our servos
int servo0 = 0;//port on PCA9685
int servo12 = 12;//port on PCA9685

//these are the values you would get from the PCA9685 Servo setter project
//see https://www.digitaltown.co.uk/projectPCA9685ServoSetter.php
int servo0LPos = 45;//servo left angle
int servo0RPos = 179;//servo right angle
int servo12LPos = 33;
int servo12RPos = 168;

//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() {
  if (currentMillis - buttonTimer >= buttonDebounce) { //are buttons debounced
    buttonTimer = currentMillis;//reset debounce timer
    //the system will only read one button at a time so checks through the buttons in order and if any are pressed
    //that button will be dealt with and any other presses at the same time ignored
    //This means only 1 debounce timer is needed.
    if (digitalRead(servo0Left) > 0) {
      setServoPos(servo0, servo0LPos);
      Serial.println("Servo 0: Left");
    } else {
      if (digitalRead(servo0Right) > 0) {
        setServoPos(servo0, servo0RPos);
        Serial.println("Servo 0: Right");
      } else {
        if (digitalRead(servo12Left) > 0) {
          setServoPos(servo12, servo12LPos);
          Serial.println("Servo 12: Left");
        } else {
          if (digitalRead(servo12Right) > 0) {
            setServoPos(servo12, servo12RPos);
            Serial.println("Servo 12: Right");
          }
        }
      }
    }
  }
}



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

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

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

  pinMode(servo0Left, INPUT);
  pinMode(servo0Right, INPUT);
  pinMode(servo12Left, INPUT);
  pinMode(servo12Right, INPUT);

}

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

}