//Universidad del Valle de Guatemala
//Electrónica Digital 1 | Proyecto Final | Brazo Mecánico
//Cristian Alonzo 21761 y Adriana Girón 21201

//Incluir las librerías correspondientes a los servomotores y a la pantalla LCD
#include <Servo.h>
#include <LiquidCrystal.h>

//Definimos las entradas de las interrupciones
#define BUTTON_PIN_1 2 
#define BUTTON_PIN_2 3
#define TIMER_INTERVAL 10000 //Intervalo de tiempo en milisegundos para el Timer (10 segundos)

//Se definen los pines de los servos
const int servoPin1 = 9; 
const int servoPin2 = 10; 
const int servoPin3 = 11; 
const int servoPin4 = 12; 

Servo servoRotar; //Servo que rota el brazo mecánico
Servo servoEstirar; //Servo que estira el brazo mecánico
Servo servoRecoger; //Servo que hace que el suba y baje el brazo
Servo servoGarra; //Servo que controla la garra del brazo

bool button1Pressed = false; 
bool button2Pressed = false; 
bool timerActive = false; //Indica que el timer no debe de ejecutarse todavía
unsigned long lastButtonPressTime = 0; //Los servos vuelven a su posión inicial después de inactividad
int initialPosition = 0; //Posición inicial para todos los servos

LiquidCrystal lcd(4, 5, 6, 7, 8, 9); //Se definen las entradas de la pantalla LCD

void setup() {
//Pines correspondientes a los servomotores
  servoRotar.attach(servoPin1);
  servoEstirar.attach(servoPin2);
  servoRecoger.attach(servoPin3);
  servoGarra.attach(servoPin4);

//Indica que los botones realizan su trabajo luego de ser presionados y soltados
  pinMode(BUTTON_PIN_1, INPUT_PULLUP);
  pinMode(BUTTON_PIN_2, INPUT_PULLUP);

//Interrupción de cambio de pin correspondiente a los botones y las acciones que ejecutan en los servos
  attachInterrupt(digitalPinToInterrupt(BUTTON_PIN_1), button1Interrupt, CHANGE);
  attachInterrupt(digitalPinToInterrupt(BUTTON_PIN_2), button2Interrupt, CHANGE);

//Los servomotores inicial todos en la misma posición que es cero
  servoRotar.write(initialPosition);
  servoEstirar.write(initialPosition);
  servoRecoger.write(initialPosition);
  servoGarra.write(initialPosition);

//Mensaje que envía la pantalla al inicio, cuando no se ha presionado ninguno de los botones
  lcd.begin(16, 2);
  lcd.print("Proyecto Final:");
  lcd.setCursor(0,1);
  lcd.print("Brazo Mecanico");
  delay(3000);
  lcd.clear();
  lcd.print("      Por:");
  delay(2000);
  lcd.clear();
  lcd.print("Cristian Alonzo");
  lcd.setCursor(0,1);
  lcd.print("y Adriana Giron");
  delay(5000);
  lcd.clear();

}

void loop() {
//Interrupción de temporizador en milisegundos, regresa los servos a cero cuando hay inactividad
  if(timerActive) {
    unsigned long currentTime = millis();
    if(currentTime - lastButtonPressTime >= TIMER_INTERVAL) {
      resetServosToInitialPosition();
      timerActive = false;
    }
  }
}

void button1Interrupt() {
//Accion del botón uno cuando es presionado, cada servomotor posee un ángulo deseado al brazo mecánico
  if (digitalRead(BUTTON_PIN_1) == HIGH) {
    if(!button1Pressed) {
      servoRotar.write(45);
      servoEstirar.write(90);
      servoRecoger.write(25);
      servoGarra.write(90);
      delay(300);
      button1Pressed = true; //Los servos se mueven solo si el botón fue presionado

//La pantalla indica las acciones que se están realizando
      lcd.clear();
      lcd.setCursor(0,0);
      lcd.print("Accion:");
      lcd.setCursor(0,1);
      lcd.print("Posicion 1");
      delay(3000);

//Se agrega la variable para indicar que debe actuar en esta parte de la programación
      resetTimer();
    }

//Si no se presiona el botón en 10 segundos, se ejecuta la interrupción de temporizador
    else {
      resetServosToInitialPosition();
      delay(3000);
      button1Pressed = false;

      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Accion: Reinicio");
      delay(2000);
    }
  }
}

void button2Interrupt() {
//Misma finalidad que el botón 1
  if(digitalRead(BUTTON_PIN_2) == HIGH) {
    if(!button2Pressed) {
      servoRotar.write(90);
      servoEstirar.write(60);
      servoRecoger.write(180);
      servoGarra.write(16);
      delay(300);
      button2Pressed = true;

      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Accion:");
      lcd.setCursor(0,1);
      lcd.print("Posicion 2");
      delay(3000);

      resetTimer();
    }
    else {
      resetServosToInitialPosition();
      delay(3000);
      button2Pressed = false;

      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Accion: Reinicio");
      delay(2000);
    }
  }
}


void resetServosToInitialPosition() {
//Regresa todos los motores a su posición inicial una vez se ejecuta la interrupción de temporizador
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("Inactividad");
  lcd.setCursor(0,1),
  lcd.print("Detectada");
  servoRotar.write(initialPosition);
  servoEstirar.write(initialPosition);
  servoRecoger.write(initialPosition);
  servoGarra.write(initialPosition);

}

void resetTimer() {
  lastButtonPressTime = millis();
  timerActive = true;
}