#include <PID_v1_bc.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>

#define ledRojo1 A0
#define ledRojo2 A1
#define ledVerde1 A2
#define ledVerde2 A3
#define pote A6
#define ACTUAL 0
#define ANTERIOR 1
#define Ts 0.004
double Kp=1, Ki=0.6, Kd=0.02, entrPID, refPID, SalidaControlador;
float Error[2], Retro, m, Entrada;
double integral = 0, dt=0, last_time, previous;

Adafruit_MPU6050 mpu;
sensors_event_t a, g, temp;

void setup()
{
  // Inicializamos las variables a las que estamos conectados
  Retro = 0;
  Entrada = 0;
  Serial.begin(115200);
  pinMode(ledRojo1, OUTPUT);
  pinMode(ledRojo2, OUTPUT);
  pinMode(ledVerde1, OUTPUT);
  pinMode(ledVerde2, OUTPUT);
  pinMode(pote, INPUT);

while (!mpu.begin())
    {
        delay(1000);
    }

}
void loop()
{
  Serial.print("\n-----------------------------------");

  Entrada = userOutput();  //Entrada
  Serial.print("\nPetición del Usuario: ");
  Serial.print(Entrada);

  MPUOutput();      //Retro
  Serial.print("\nValor Retro: ");
  Serial.println(Retro);
  
  calcError();
  Serial.print("\nValor Error: ");
  Serial.println(Error[ACTUAL]);

  double now = millis();
  dt = Ts;
  last_time = now;

  double actual = Retro;
  SalidaControlador = pid(Error[ACTUAL]);

  // Escribimos el valor de salida
  Serial.print("\nSalida del controlador: ");
  Serial.print(SalidaControlador);

  droneControl(SalidaControlador);
  
  delay(1000);

}
// (Elemento de Medición) Lee por puerto serie los bits que envía el MPU6050 y los convierte en una inclinación en grados
// Utilizamos un Ping para su representación, directamente varía el ángulo con respecto a la distancia ya que
// es solidaria con la aceleración y la velocidad angular.

double pid(double error)
{
  double proportional = error;
  integral += error * dt;
  double derivative = (error - previous) / dt;
  previous = error;
  
    // Limito los valores
    if (integral > 300)
    {
        integral = 300;
    }
    else if (integral < -300)
    {
        integral = -300;
    }
  
  double output = (Kp * proportional) + (Ki * integral) + (Kd * derivative);
  return output;
}

void MPUOutput()
{
   mpu.getEvent(&a, &g, &temp);
   Retro = g.gyro.x*500/8.7266464;
}

// (Entrada) Lee los pulsos enviados por radiofrecuencia por el usuario, los interpreta según el tiempo en ON y OFF y los transforma en una inclinación en grados
// Utilizamos un pote para su representación, como solo estamos simulando una de las rotaciones, requerimos uno
// solo
float userOutput()
{
  float valor = analogRead(pote);
  valor = map(valor,0,1023,0,255);
  return valor;
}

void calcError() {
    // Actualizo los movimientos del ciclo anterior
    Error[ANTERIOR] = Error[ACTUAL];
    Error[ACTUAL] = Entrada - Retro;
}

// (Salida a los actuadores) Según la entrada recibida y su retroalimentación, modifica la velocidad de los motores según corresponda
// para su estabilización (tiene en cuenta el PID del Pitch y del Roll)
// Utilizamos 4 leds para la representación de los motores, para no estudiar cómo se controla un dron en su
// totalidad, representamos el giro como 2 aumentando su luminosidad y los otros 2 disminuyendola.
void droneControl(float SalidaControlador)
{
  float rojo;
  float verde;

  if(SalidaControlador>255/2)
  {

    rojo = 255/2 + SalidaControlador/2;
    verde = 255/2 - SalidaControlador/2;
  }
  else
  {
    verde = 255/2 + SalidaControlador/2;
    rojo = 255/2 - SalidaControlador/2;
  }

  analogWrite(ledRojo1,rojo);
  analogWrite(ledRojo2,rojo);
  analogWrite(ledVerde1,verde);
  analogWrite(ledVerde2,verde);

  Serial.print("\nMRojo: ");
  Serial.print(rojo);
  Serial.print(" | MVerde: ");
  Serial.print(verde);
}