/* PCA9685ServoSetterV1

   Goals

   To be able to set a servo to 90 dgrees ready for baseboard install.
   Be able to move the servo side to side adjusting the distance to get the correct figures for the points.
   Code to be written as a state machine without delay()

   PCA9685 board
   button to set to 90 degrees....or into side to side mode
   button to select left/right direction adjustment
   2 buttons for adjusters (Lower and Higher values)
   4 x 4.5k OHM resistors for Pull Down on buttons


   Pin Connections

   SDA/SCL connections for PCA9685

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

  pin 8 mode swap ... button with 4.7K pull down resistor
  pin 9 left/right adjustment swap... button with 4.7K pull down resistor
  pin 10 low adjust... button with 4.7K pull down resistor
  pin 11 high adjustment... button with 4.7K pull down resistor


*/

#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 modePin = 8; // sets if servo is moved to 90 degrees or sweeping left to right.
const int adjustPin = 9;//sets if we are adjusting the anticlockwise (nearer to zero) or clockwise (nearer to 180)
const int lowAdjPin = 10;
const int highAdjPin = 11;

// our servo
int myServo = 0;//we will always be testing on pins 0 on PCA9685

//System variables
byte currentMode = 0; //0 sets to 90 degrees 1: sweep mode
byte adjustSide = 0; //0 = Low...nearest to Zero 1 = High side nearest to 180
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

unsigned long servoSweepTimer; //stores when next sweep can start
int sweepTimer = 2500;//set to 2.5 secs between sweeps
byte sweepDirection;//0 == anticlockwise 1 clockwise
int sweepLowLimit = 90;//starts in central position
int sweepHighLimit = 90;//starts in central position

//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
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(modePin) > 0) {
      //Serial.println("modePin");
      if (currentMode > 0) {
        currentMode = 0;//move to 90 degrees
        Serial.println("Move to 90");
      } else {
        currentMode = 1;//sweep mode
        Serial.println("Sweep Mode");
      }
    } else {
      if (digitalRead(adjustPin) > 0) {
        //Serial.println("adjustPin");
        if (adjustSide > 0) {
          adjustSide = 0;
          Serial.println("adjust Low");
        } else {
          adjustSide = 1;
          Serial.println("adjust High");
        }
      } else {
        if (digitalRead(lowAdjPin) > 0) {
          //Serial.println("lowAdjPin");
          if (adjustSide > 0) {
            sweepHighLimit--;
            Serial.print("High Set: ");
            Serial.println(sweepHighLimit);
          } else {
            sweepLowLimit--;
            Serial.print("Low set: ");
            Serial.println(sweepLowLimit);
          }
        } else {
          if (digitalRead(highAdjPin) > 0) {
            //Serial.println("highAdjPin");
            if (adjustSide > 0) {
              sweepHighLimit++;
              Serial.print("High Set: ");
              Serial.println(sweepHighLimit);
            } else {
              sweepLowLimit++;
              Serial.print("Low set: ");
              Serial.println(sweepLowLimit);
            }
          }
        }
      }
    }
  }
}

//Checks if the servo needs to move
void servoMode() {
  if (currentMode < 1) {
    setServoPos(myServo, 90);//set servo to centre postion
  } else { //normal sweep mode
    if (currentMillis - servoSweepTimer >= sweepTimer) {
      
      servoSweepTimer = currentMillis;//reset for next sweep
      
      if (sweepDirection  < 1) { //move to anticlockwise limit
        setServoPos(myServo, sweepLowLimit);
        Serial.print("M Low: ");
        Serial.println(sweepLowLimit);
        sweepDirection = 1;
      } else { //move to clockwise limit
        setServoPos(myServo, sweepHighLimit);
        Serial.print("M High: ");
        Serial.println(sweepHighLimit);
        sweepDirection = 0;
      }
    }
  }
}

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(modePin, INPUT);
  pinMode(adjustPin, INPUT);
  pinMode(lowAdjPin, INPUT);
  pinMode(highAdjPin, INPUT);

}

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