#include<EEPROM.h>

//definicion de pines
int resetButton = 27;
int motorPWM = 14; // Pin del PWM para controlar la velocidad del motor
int motorIN1 = 12; // Pin de dirección 1 del motor
int motorIN2 = 13; // Pin de dirección 2 del motor
int potDeseadaPin = 34; // Pin del potenciómetro para la posición deseada
int potActualPin = 35; // Pin del potenciómetro para la posición actual

//pid variables
double kp = 1.0; // Ganancia proporcional del controlador PID
double ki = 0.1; // Ganancia integral del controlador PID
double kd = 0.02; // Ganancia derivativa del controlador PID
unsigned long lastUpdateTime = 0; // Variable para el tiempo de la última actualización
int lastError = 0; // Variable para almacenar el error en el ciclo anterior
double integral = 0; // Variable para la componente integral del controlador PID

//variables de calibracion
int minValue = 0; //valor minimo del potenciometro calibrado 
int maxValue = 4095; //valor maximo del potenciometro calibrado 
bool calibrated = false; //flag para saber si el potenciometro ha sido calibrado 

void guardarCalibracionActual(){
  int address = 0; // Dirección de memoria EEPROM para almacenar los valores
  EEPROM.write(address, (uint8_t)(minValue & 0xFF)); // Almacena el valor de minValue en la dirección de memoria
  address++;
  EEPROM.write(address, (uint8_t)((minValue >> 8) & 0xFF));
  address++;
  EEPROM.write(address, (uint8_t)(maxValue & 0xFF)); // Almacena el valor de maxValue en la siguiente dirección de memoria
  address++;
  EEPROM.write(address, (uint8_t)((maxValue >> 8) & 0xFF));
  address++;
}

void cargarCalibracionActual(){
  int address = 0; // Dirección de memoria EEPROM para cargar los valores
  int lowByte;
  int highByte;
  lowByte = EEPROM.read(address); // Carga el valor de minValue desde la dirección de memoria
  address++;
  highByte = EEPROM.read(address);
  minValue = lowByte | (highByte << 8);
  address++;
  lowByte = EEPROM.read(address); // Carga el valor de maxValue desde la siguiente dirección de memoria
  address++;
  highByte = EEPROM.read(address);
  maxValue = lowByte | (highByte << 8);
}

void setup() {
  pinMode(motorPWM, OUTPUT);
  pinMode(motorIN1, OUTPUT);
  pinMode(motorIN2, OUTPUT);
  pinMode(potDeseadaPin, INPUT);
  pinMode(potActualPin, INPUT);
  pinMode(resetButton,INPUT_PULLUP);
  Serial.begin(9600);
  cargarCalibracionActual(); // Verifica si hay valores almacenados en la EEPROM y carga los valores calibrados
  if (!calibrated){
    calibrarPotenciometroActual();
    guardarCalibracionActual(); // Guarda los valores calibrados en la EEPROM
  }
}

void calibrarPotenciometroActual() {

  int minVal = 4095; // Inicializar minVal con un valor alto

  int maxVal = 0; // Inicializar maxVal con un valor bajo

  digitalWrite(motorIN1, LOW); // IN1 = 0
  digitalWrite(motorIN2, HIGH); // IN2 = 1
  analogWrite(motorPWM, 160); // PWM = 160

  delay(500); // Esperar un segundo para que el potenciómetro se estabilice

  for (int i = 0; i < 100; i++) {

    int val = analogRead(potActualPin);

    if (val < minVal) {
      minVal = val;
    }

    delay(10);
  }

  digitalWrite(motorIN1, HIGH); // IN1 = 1
  digitalWrite(motorIN2, LOW); // IN2 = 0

  delay(500); // Esperar un segundo para que el potenciómetro se estabilice

  for (int i = 0; i < 100; i++) {

    int val = analogRead(potActualPin);

    if (val > maxVal) {
      maxVal = val;
    }

    delay(10);
  }

  minValue = minVal;
  // Set minValue as the minimum value obtained
  
  maxValue = maxVal;
  // Set maxValue as the maximum value obtained
  
  calibrated = true;

}

void loop(){  
  if (digitalRead(resetButton) == LOW) { // Si se presiona el botón
    EEPROM.write(0, 0); // Restablecer el valor de minValue en la dirección 0 de la EEPROM
    EEPROM.write(1, 0); // Restablecer el valor de maxValue en la dirección 1 de la EEPROM
    calibrated = false; // Establecer calibrated como falso para indicar que el potenciómetro debe calibrarse nuevamente
    Serial.println("Valores de la EEPROM restablecidos");
    delay(1000); // Esperar un segundo para evitar múltiples restablecimientos con un solo botón
  }
  if (!calibrated){
    calibrarPotenciometroActual();
  }
  if (millis() - lastUpdateTime >= 50) {
    lastUpdateTime = millis();
    int posicionDeseada = map(analogRead(potDeseadaPin), 0, 4095, 0, 255);
    int posicionActual = map(analogRead(potActualPin), minValue, maxValue, 0, 255);
    
    double error = posicionDeseada - posicionActual;
    integral = integral + error;
    double derivativa = error - lastError;
    lastError = error;
    
    double correccion = (kp * error) + (ki * integral) + (kd * derivativa);
    int pwmValue = abs(correccion);
    
    if (correccion > 0) {
      analogWrite(motorPWM, pwmValue);
      digitalWrite(motorIN1, HIGH);
      digitalWrite(motorIN2, LOW);
    } else if (correccion < 0) {
      analogWrite(motorPWM, pwmValue);
      digitalWrite(motorIN1, LOW);
      digitalWrite(motorIN2, HIGH);
    } else {
      analogWrite(motorPWM, 0);
      digitalWrite(motorIN1, LOW);
      digitalWrite(motorIN2, LOW);
    }
    Serial.print("Potenciómetro actual: ");
    Serial.print(posicionActual);
    Serial.print(" | Valor PWM: ");
    Serial.println(pwmValue);
  }
}